Merge pull request ros-industrial#115 from Jmeyer1292/issue#114_start…

…_tolerance_check Issue#114 - Action Server withinGoalConstraints check removal
JeremyZoss · Jun 28, 2015 · 32d26d0 · 32d26d0
2 parents bcad8cf + 26cad32
commit 32d26d0
Showing 1 changed file with 8 additions and 20 deletions.
diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp
@@ -133,28 +133,16 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
         ROS_WARN("Received new goal, canceling current goal");
         abortGoal();
       }
+
+      gh.setAccepted();
+      active_goal_ = gh;
+      has_active_goal_ = true;
 
-      // Sends the trajectory along to the controller
-      if (withinGoalConstraints(last_trajectory_state_, gh.getGoal()->trajectory))
-      {
-
-        ROS_INFO_STREAM("Already within goal constraints, setting goal succeeded");
-        gh.setAccepted();
-        gh.setSucceeded();
-        has_active_goal_ = false;
+      ROS_INFO("Publishing trajectory");
 
-      }
-      else
-      {
-        gh.setAccepted();
-        active_goal_ = gh;
-        has_active_goal_ = true;
-
-        ROS_INFO("Publishing trajectory");
-
-        current_traj_ = active_goal_.getGoal()->trajectory;
-        pub_trajectory_command_.publish(current_traj_);
-      }
+      current_traj_ = active_goal_.getGoal()->trajectory;
+      pub_trajectory_command_.publish(current_traj_);
+
     }
     else
     {