00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <ros/ros.h>
00039
00040 #include <tf/tf.h>
00041 #include <tf/transform_listener.h>
00042 #include <urdf/model.h>
00043 #include <angles/angles.h>
00044
00045 #include <actionlib/client/action_client.h>
00046 #include <control_msgs/FollowJointTrajectoryAction.h>
00047
00048 #include <actionlib/server/simple_action_server.h>
00049 #include <arm_navigation_msgs/MoveArmStatistics.h>
00050 #include <arm_navigation_msgs/MoveArmAction.h>
00051
00052 #include <trajectory_msgs/JointTrajectory.h>
00053 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00054 #include <kinematics_msgs/GetPositionFK.h>
00055
00056 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00057 #include <arm_navigation_msgs/Shape.h>
00058 #include <arm_navigation_msgs/DisplayTrajectory.h>
00059 #include <arm_navigation_msgs/GetMotionPlan.h>
00060 #include <arm_navigation_msgs/convert_messages.h>
00061 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00062
00063 #include <visualization_msgs/Marker.h>
00064
00065 #include <planning_environment/util/construct_object.h>
00066 #include <arm_navigation_msgs/utils.h>
00067 #include <geometric_shapes/bodies.h>
00068
00069 #include <visualization_msgs/MarkerArray.h>
00070
00071 #include <planning_environment/models/collision_models.h>
00072 #include <planning_environment/models/model_utils.h>
00073 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00074
00075 #include <arm_navigation_msgs/GetRobotState.h>
00076
00077 #include <actionlib/client/simple_action_client.h>
00078 #include <actionlib/client/simple_client_goal_state.h>
00079
00080 #include <std_msgs/Bool.h>
00081
00082 #include <valarray>
00083 #include <algorithm>
00084 #include <cstdlib>
00085
00086 typedef actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction> JointExecutorActionClient;
00087
00088 namespace move_arm
00089 {
00090
00091 enum MoveArmState {
00092 PLANNING,
00093 START_CONTROL,
00094 VISUALIZE_PLAN,
00095 MONITOR
00096 };
00097
00098 enum ControllerStatus {
00099 QUEUED,
00100 ACTIVE,
00101 SUCCESS,
00102 FAILED
00103 };
00104
00105 enum EnvironmentServerChecks{
00106 COLLISION_TEST = 1,
00107 PATH_CONSTRAINTS_TEST = 2,
00108 GOAL_CONSTRAINTS_TEST = 4,
00109 JOINT_LIMITS_TEST = 8,
00110 CHECK_FULL_TRAJECTORY = 16
00111 };
00112
00113 typedef struct{
00114 bool accept_partial_plans;
00115 bool accept_invalid_goals;
00116 bool disable_ik;
00117 bool disable_collision_monitoring;
00118 bool is_pose_goal;
00119 double allowed_planning_time;
00120 std::string planner_service_name;
00121 } MoveArmParameters;
00122
00123 static const std::string ARM_IK_NAME = "arm_ik";
00124 static const std::string TRAJECTORY_FILTER = "/trajectory_filter_server/filter_trajectory_with_constraints";
00125 static const std::string DISPLAY_PATH_PUB_TOPIC = "display_path";
00126 static const std::string DISPLAY_JOINT_GOAL_PUB_TOPIC = "display_joint_goal";
00127
00128
00129
00130 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00131 static const double MIN_TRAJECTORY_MONITORING_FREQUENCY = 1.0;
00132 static const double MAX_TRAJECTORY_MONITORING_FREQUENCY = 100.0;
00133
00134 class MoveArm
00135 {
00136 public:
00137
00138 MoveArm(const std::string &group_name) :
00139 group_(group_name),
00140 private_handle_("~")
00141 {
00142 private_handle_.param<double>("move_arm_frequency",move_arm_frequency_, 50.0);
00143 private_handle_.param<double>("trajectory_filter_allowed_time",trajectory_filter_allowed_time_, 2.0);
00144 private_handle_.param<double>("ik_allowed_time",ik_allowed_time_, 2.0);
00145
00146 private_handle_.param<bool>("publish_stats",publish_stats_, true);
00147
00148 planning_scene_state_ = NULL;
00149
00150 collision_models_ = new planning_environment::CollisionModels("robot_description");
00151
00152 num_planning_attempts_ = 0;
00153 state_ = PLANNING;
00154
00155 ik_client_ = root_handle_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(ARM_IK_NAME);
00156 allowed_contact_regions_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("allowed_contact_regions_array", 128);
00157 filter_trajectory_client_ = root_handle_.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(TRAJECTORY_FILTER);
00158 vis_marker_publisher_ = root_handle_.advertise<visualization_msgs::Marker>("move_" + group_name+"_markers", 128);
00159 vis_marker_array_publisher_ = root_handle_.advertise<visualization_msgs::MarkerArray>("move_" + group_name+"_markers_array", 128);
00160 get_state_client_ = root_handle_.serviceClient<arm_navigation_msgs::GetRobotState>("/environment_server/get_robot_state");
00161
00162
00163 arm_ik_initialized_ = false;
00164 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00165 set_planning_scene_diff_client_ = root_handle_.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
00166
00167 ros::service::waitForService(TRAJECTORY_FILTER);
00168
00169 action_server_.reset(new actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction>(root_handle_, "move_" + group_name, boost::bind(&MoveArm::execute, this, _1), false));
00170 action_server_->start();
00171
00172 display_path_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>(DISPLAY_PATH_PUB_TOPIC, 1, true);
00173 display_joint_goal_publisher_ = root_handle_.advertise<arm_navigation_msgs::DisplayTrajectory>(DISPLAY_JOINT_GOAL_PUB_TOPIC, 1, true);
00174 stats_publisher_ = private_handle_.advertise<arm_navigation_msgs::MoveArmStatistics>("statistics",1,true);
00175 }
00176 virtual ~MoveArm()
00177 {
00178 revertPlanningScene();
00179 delete collision_models_;
00180 }
00181
00182 bool configure()
00183 {
00184 if(!initializeControllerInterface())
00185 {
00186 ROS_ERROR("Could not initialize controller interface");
00187 return false;
00188 }
00189 if (group_.empty())
00190 {
00191 ROS_ERROR("No 'group' parameter specified. Without the name of the group of joints to plan for, action cannot start");
00192 return false;
00193 }
00194 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_->getKinematicModel()->getModelGroup(group_);
00195 if(joint_model_group == NULL) {
00196 ROS_WARN_STREAM("No joint group " << group_);
00197 return false;
00198 }
00199 group_joint_names_ = joint_model_group->getJointModelNames();
00200 group_link_names_ = joint_model_group->getGroupLinkNames();
00201 return true;
00202 }
00203
00204 private:
00205
00209 bool convertPoseGoalToJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00210 {
00211 if(!arm_ik_initialized_)
00212 {
00213 if(!ros::service::waitForService(ARM_IK_NAME,ros::Duration(1.0)))
00214 {
00215 ROS_WARN("Inverse kinematics service is unavailable");
00216 return false;
00217 }
00218 else
00219 {
00220 arm_ik_initialized_ = true;
00221 }
00222 }
00223
00224
00225 ROS_DEBUG("Acting on goal to pose ...");
00226 ROS_DEBUG("Position constraint: %f %f %f",
00227 req.motion_plan_request.goal_constraints.position_constraints[0].position.x,
00228 req.motion_plan_request.goal_constraints.position_constraints[0].position.y,
00229 req.motion_plan_request.goal_constraints.position_constraints[0].position.z);
00230 ROS_DEBUG("Orientation constraint: %f %f %f %f",
00231 req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x,
00232 req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y,
00233 req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z,
00234 req.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w);
00235
00236 geometry_msgs::PoseStamped tpose = arm_navigation_msgs::poseConstraintsToPoseStamped(req.motion_plan_request.goal_constraints.position_constraints[0],
00237 req.motion_plan_request.goal_constraints.orientation_constraints[0]);
00238 std::string link_name = req.motion_plan_request.goal_constraints.position_constraints[0].link_name;
00239 sensor_msgs::JointState solution;
00240
00241 ROS_DEBUG("IK request");
00242 ROS_DEBUG("link_name : %s",link_name.c_str());
00243 ROS_DEBUG("frame_id : %s",tpose.header.frame_id.c_str());
00244 ROS_DEBUG("position : (%f,%f,%f)",tpose.pose.position.x,tpose.pose.position.y,tpose.pose.position.z);
00245 ROS_DEBUG("orientation : (%f,%f,%f,%f)",tpose.pose.orientation.x,tpose.pose.orientation.y,tpose.pose.orientation.z,tpose.pose.orientation.w);
00246 ROS_DEBUG(" ");
00247 if (computeIK(tpose,
00248 link_name,
00249 solution))
00250 {
00251 std::map<std::string, double> joint_values;
00252 for (unsigned int i = 0 ; i < solution.name.size() ; ++i)
00253 {
00254 arm_navigation_msgs::JointConstraint jc;
00255 jc.joint_name = solution.name[i];
00256 jc.position = solution.position[i];
00257 jc.tolerance_below = 0.01;
00258 jc.tolerance_above = 0.01;
00259 req.motion_plan_request.goal_constraints.joint_constraints.push_back(jc);
00260 joint_values[jc.joint_name] = jc.position;
00261 }
00262 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00263 resetToStartState(planning_scene_state_);
00264 planning_scene_state_->setKinematicState(joint_values);
00265 if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00266 group_joint_names_,
00267 error_code,
00268 original_request_.motion_plan_request.goal_constraints,
00269 original_request_.motion_plan_request.path_constraints,
00270 true))
00271 {
00272 ROS_INFO("IK returned joint state for goal that doesn't seem to be valid");
00273 if(error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00274 ROS_WARN("IK solution doesn't obey goal constraints");
00275 } else if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00276 ROS_WARN("IK solution in collision");
00277 } else {
00278 ROS_WARN_STREAM("Some other problem with ik solution " << error_code.val);
00279 }
00280 }
00281 req.motion_plan_request.goal_constraints.position_constraints.clear();
00282 req.motion_plan_request.goal_constraints.orientation_constraints.clear();
00283 return true;
00284 }
00285 else
00286 return false;
00287 }
00288
00289 bool computeIK(const geometry_msgs::PoseStamped &pose_stamped_msg,
00290 const std::string &link_name,
00291 sensor_msgs::JointState &solution)
00292 {
00293 kinematics_msgs::GetConstraintAwarePositionIK::Request request;
00294 kinematics_msgs::GetConstraintAwarePositionIK::Response response;
00295
00296 request.ik_request.pose_stamped = pose_stamped_msg;
00297 request.ik_request.robot_state = original_request_.motion_plan_request.start_state;
00298 request.ik_request.ik_seed_state = request.ik_request.robot_state;
00299 request.ik_request.ik_link_name = link_name;
00300 request.timeout = ros::Duration(ik_allowed_time_);
00301 request.constraints = original_request_.motion_plan_request.goal_constraints;
00302 if (ik_client_.call(request, response))
00303 {
00304 move_arm_action_result_.error_code = response.error_code;
00305 if(response.error_code.val != response.error_code.SUCCESS)
00306 {
00307 ROS_ERROR("IK Solution not found, IK returned with error_code: %d",response.error_code.val);
00308 return false;
00309 }
00310 solution = response.solution.joint_state;
00311 if (solution.position.size() != group_joint_names_.size())
00312 {
00313 ROS_ERROR("Incorrect number of elements in IK output.");
00314 return false;
00315 }
00316 for(unsigned int i = 0; i < solution.position.size() ; ++i)
00317 ROS_DEBUG("IK[%d] = %f", (int)i, solution.position[i]);
00318 }
00319 else
00320 {
00321 ROS_ERROR("IK service failed");
00322 return false;
00323 }
00324 return true;
00325 }
00329
00333 bool filterTrajectory(const trajectory_msgs::JointTrajectory &trajectory_in,
00334 trajectory_msgs::JointTrajectory &trajectory_out)
00335 {
00336 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request req;
00337 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response res;
00338 fillTrajectoryMsg(trajectory_in, req.trajectory);
00339
00340 if(trajectory_filter_allowed_time_ == 0.0)
00341 {
00342 trajectory_out = req.trajectory;
00343 return true;
00344 }
00345 resetToStartState(planning_scene_state_);
00346 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
00347 ros::Time::now(),
00348 collision_models_->getWorldFrameId(),
00349 req.start_state);
00350 req.group_name = group_;
00351 req.path_constraints = original_request_.motion_plan_request.path_constraints;
00352 req.goal_constraints = original_request_.motion_plan_request.goal_constraints;
00353 req.allowed_time = ros::Duration(trajectory_filter_allowed_time_);
00354 ros::Time smoothing_time = ros::Time::now();
00355 if(filter_trajectory_client_.call(req,res))
00356 {
00357 move_arm_stats_.trajectory_duration = (res.trajectory.points.back().time_from_start-res.trajectory.points.front().time_from_start).toSec();
00358 move_arm_stats_.smoothing_time = (ros::Time::now()-smoothing_time).toSec();
00359 trajectory_out = res.trajectory;
00360 return true;
00361 }
00362 else
00363 {
00364 ROS_ERROR("Service call to filter trajectory failed.");
00365 return false;
00366 }
00367 }
00368
00372
00373 void discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory,
00374 trajectory_msgs::JointTrajectory &trajectory_out,
00375 const double &trajectory_discretization)
00376 {
00377 trajectory_out.joint_names = trajectory.joint_names;
00378 for(unsigned int i=1; i < trajectory.points.size(); i++)
00379 {
00380 double diff = 0.0;
00381 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00382 {
00383 double start = trajectory.points[i-1].positions[j];
00384 double end = trajectory.points[i].positions[j];
00385 if(fabs(end-start) > diff)
00386 diff = fabs(end-start);
00387 }
00388 int num_intervals =(int) (diff/trajectory_discretization+1.0);
00389
00390 for(unsigned int k=0; k < (unsigned int) num_intervals; k++)
00391 {
00392 trajectory_msgs::JointTrajectoryPoint point;
00393 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00394 {
00395 double start = trajectory.points[i-1].positions[j];
00396 double end = trajectory.points[i].positions[j];
00397 point.positions.push_back(start + (end-start)*k/num_intervals);
00398 }
00399 point.time_from_start = ros::Duration(trajectory.points[i].time_from_start.toSec() + k* (trajectory.points[i].time_from_start - trajectory.points[i-1].time_from_start).toSec()/num_intervals);
00400 trajectory_out.points.push_back(point);
00401 }
00402 }
00403 trajectory_out.points.push_back(trajectory.points.back());
00404 }
00408
00412 bool isPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00413 {
00414 if (req.motion_plan_request.goal_constraints.joint_constraints.empty() &&
00415 req.motion_plan_request.goal_constraints.position_constraints.size() == 1 &&
00416 req.motion_plan_request.goal_constraints.orientation_constraints.size() == 1)
00417 return true;
00418 else
00419 return false;
00420 }
00421 bool hasPoseGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00422 {
00423 if (req.motion_plan_request.goal_constraints.position_constraints.size() >= 1 &&
00424 req.motion_plan_request.goal_constraints.orientation_constraints.size() >= 1)
00425 return true;
00426 else
00427 return false;
00428 }
00429 bool isJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
00430 {
00431 if (req.motion_plan_request.goal_constraints.position_constraints.empty() &&
00432 req.motion_plan_request.goal_constraints.orientation_constraints.empty() &&
00433 !req.motion_plan_request.goal_constraints.joint_constraints.empty())
00434 return true;
00435 else
00436 return false;
00437 }
00438
00439
00440 bool isExecutionSafe() {
00441 return true;
00442 }
00443
00444 bool getRobotState(planning_models::KinematicState* state)
00445 {
00446 arm_navigation_msgs::GetRobotState::Request req;
00447 arm_navigation_msgs::GetRobotState::Response res;
00448 if(get_state_client_.call(req,res))
00449 {
00450 planning_environment::setRobotStateAndComputeTransforms(res.robot_state, *state);
00451 }
00452 else
00453 {
00454 ROS_ERROR("Service call to get robot state failed on %s",
00455 get_state_client_.getService().c_str());
00456 return false;
00457 }
00458 return true;
00459 }
00463
00467 void moveArmGoalToPlannerRequest(const arm_navigation_msgs::MoveArmGoalConstPtr& goal,
00468 arm_navigation_msgs::GetMotionPlan::Request &req)
00469 {
00470 req.motion_plan_request.workspace_parameters.workspace_region_pose.header.stamp = ros::Time::now();
00471 req.motion_plan_request = goal->motion_plan_request;
00472
00473 move_arm_parameters_.accept_partial_plans = goal->accept_partial_plans;
00474 move_arm_parameters_.accept_invalid_goals = goal->accept_invalid_goals;
00475 move_arm_parameters_.disable_ik = goal->disable_ik;
00476 move_arm_parameters_.disable_collision_monitoring = goal->disable_collision_monitoring;
00477 move_arm_parameters_.allowed_planning_time = goal->motion_plan_request.allowed_planning_time.toSec();
00478 move_arm_parameters_.planner_service_name = goal->planner_service_name;
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489 }
00490 bool doPrePlanningChecks(arm_navigation_msgs::GetMotionPlan::Request &req,
00491 arm_navigation_msgs::GetMotionPlan::Response &res)
00492 {
00493 arm_navigation_msgs::Constraints empty_goal_constraints;
00494 if(planning_scene_state_ == NULL) {
00495 ROS_INFO("Can't do pre-planning checks without planning state");
00496 }
00497 resetToStartState(planning_scene_state_);
00498 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00499 if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00500 group_joint_names_,
00501 error_code,
00502 empty_goal_constraints,
00503 original_request_.motion_plan_request.path_constraints,
00504 true)) {
00505 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00506 move_arm_action_result_.error_code.val = error_code.START_STATE_IN_COLLISION;
00507 collision_models_->getAllCollisionsForState(*planning_scene_state_,
00508 move_arm_action_result_.contacts);
00509 ROS_ERROR("Starting state is in collision, can't plan");
00510 visualization_msgs::MarkerArray arr;
00511 std_msgs::ColorRGBA point_color_;
00512 point_color_.a = 1.0;
00513 point_color_.r = 1.0;
00514 point_color_.g = .8;
00515 point_color_.b = 0.04;
00516
00517 collision_models_->getAllCollisionPointMarkers(*planning_scene_state_,
00518 arr,
00519 point_color_,
00520 ros::Duration(0.0));
00521 std_msgs::ColorRGBA col;
00522 col.a = .9;
00523 col.r = 1.0;
00524 col.b = 1.0;
00525 col.g = 0.0;
00526
00527
00528
00529
00530
00531
00532 vis_marker_array_publisher_.publish(arr);
00533 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00534 move_arm_action_result_.error_code.val = error_code.START_STATE_VIOLATES_PATH_CONSTRAINTS;
00535 ROS_ERROR("Starting state violated path constraints, can't plan");;
00536 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00537 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED;;
00538 ROS_ERROR("Start state violates joint limits, can't plan.");
00539 }
00540 ROS_INFO("Setting aborted because start state invalid");
00541 action_server_->setAborted(move_arm_action_result_);
00542 return false;
00543 }
00544
00545 if (!move_arm_parameters_.disable_ik && isPoseGoal(req)) {
00546 ROS_DEBUG("Planning to a pose goal");
00547 if(!convertPoseGoalToJointGoal(req)) {
00548 ROS_INFO("Setting aborted because ik failed");
00549 action_server_->setAborted(move_arm_action_result_);
00550 return false;
00551 }
00552 }
00553
00554 if(!hasPoseGoal(req)) {
00555 arm_navigation_msgs::RobotState empty_state;
00556 empty_state.joint_state = arm_navigation_msgs::jointConstraintsToJointState(req.motion_plan_request.goal_constraints.joint_constraints);
00557 planning_environment::setRobotStateAndComputeTransforms(empty_state, *planning_scene_state_);
00558 arm_navigation_msgs::Constraints empty_constraints;
00559 if(!collision_models_->isKinematicStateValid(*planning_scene_state_,
00560 group_joint_names_,
00561 error_code,
00562 original_request_.motion_plan_request.goal_constraints,
00563 original_request_.motion_plan_request.path_constraints,
00564 true)) {
00565 if(error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00566 ROS_ERROR("Will not plan to requested joint goal since it violates joint limits constraints");
00567 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.JOINT_LIMITS_VIOLATED;
00568 } else if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00569 ROS_ERROR("Will not plan to requested joint goal since it is in collision");
00570 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_IN_COLLISION;
00571 collision_models_->getAllCollisionsForState(*planning_scene_state_,
00572 move_arm_action_result_.contacts);
00573 } else if(error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00574 ROS_ERROR("Will not plan to requested joint goal since it violates goal constraints");
00575 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00576 } else if(error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00577 ROS_ERROR("Will not plan to requested joint goal since it violates path constraints");
00578 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.GOAL_VIOLATES_PATH_CONSTRAINTS;
00579 } else {
00580 ROS_INFO_STREAM("Will not plan to request joint goal due to error code " << error_code.val);
00581 }
00582 ROS_INFO_STREAM("Setting aborted becuase joint goal is problematic");
00583 action_server_->setAborted(move_arm_action_result_);
00584 return false;
00585 }
00586 }
00587 return true;
00588 }
00589
00590 bool createPlan(arm_navigation_msgs::GetMotionPlan::Request &req,
00591 arm_navigation_msgs::GetMotionPlan::Response &res)
00592 {
00593 while(!ros::service::waitForService(move_arm_parameters_.planner_service_name, ros::Duration(1.0))) {
00594 ROS_INFO_STREAM("Waiting for requested service " << move_arm_parameters_.planner_service_name);
00595 }
00596 ros::ServiceClient planning_client = root_handle_.serviceClient<arm_navigation_msgs::GetMotionPlan>(move_arm_parameters_.planner_service_name);
00597 move_arm_stats_.planner_service_name = move_arm_parameters_.planner_service_name;
00598 ROS_DEBUG("Issuing request for motion plan");
00599
00600 if (planning_client.call(req, res))
00601 {
00602 if (res.trajectory.joint_trajectory.points.empty())
00603 {
00604 ROS_WARN("Motion planner was unable to plan a path to goal");
00605 return false;
00606 }
00607 ROS_DEBUG("Motion planning succeeded");
00608 return true;
00609 }
00610 else
00611 {
00612 ROS_ERROR("Motion planning service failed on %s",planning_client.getService().c_str());
00613 return false;
00614 }
00615 }
00619
00623 bool initializeControllerInterface()
00624 {
00625 std::string controller_action_name;
00626 private_handle_.param<std::string>("controller_action_name", controller_action_name, "action");
00627 ROS_INFO("Connecting to controller using action: %s",controller_action_name.c_str());
00628 controller_action_client_ = new JointExecutorActionClient(controller_action_name);
00629 if(!controller_action_client_) {
00630 ROS_ERROR("Controller action client hasn't been initialized yet");
00631 return false;
00632 }
00633 while(!controller_action_client_->waitForActionServerToStart(ros::Duration(1.0))){
00634 ROS_INFO("Waiting for the joint_trajectory_action server to come up.");
00635 if(!root_handle_.ok()) {
00636 return false;
00637 }
00638 }
00639 ROS_INFO("Connected to the controller");
00640 return true;
00641 }
00642
00643
00644 bool stopTrajectory()
00645 {
00646 if (controller_goal_handle_.isExpired())
00647 ROS_ERROR("Expired goal handle. controller_status = %d", controller_status_);
00648 else
00649 controller_goal_handle_.cancel();
00650 return true;
00651 }
00652 bool sendTrajectory(trajectory_msgs::JointTrajectory ¤t_trajectory)
00653 {
00654 current_trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2);
00655
00656 control_msgs::FollowJointTrajectoryGoal goal;
00657 goal.trajectory = current_trajectory;
00658
00659 ROS_DEBUG("Sending trajectory with %d points and timestamp: %f",(int)goal.trajectory.points.size(),goal.trajectory.header.stamp.toSec());
00660 for(unsigned int i=0; i < goal.trajectory.joint_names.size(); i++)
00661 ROS_DEBUG("Joint: %d name: %s",i,goal.trajectory.joint_names[i].c_str());
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681
00682 controller_goal_handle_ = controller_action_client_->sendGoal(goal,boost::bind(&MoveArm::controllerTransitionCallback, this, _1));
00683
00684 controller_status_ = QUEUED;
00685
00686 return true;
00687 }
00688
00689 void controllerTransitionCallback(JointExecutorActionClient::GoalHandle gh)
00690 {
00691 if(gh != controller_goal_handle_)
00692 return;
00693 actionlib::CommState comm_state = gh.getCommState();
00694 switch( comm_state.state_)
00695 {
00696 case actionlib::CommState::WAITING_FOR_GOAL_ACK:
00697 case actionlib::CommState::PENDING:
00698 case actionlib::CommState::RECALLING:
00699 controller_status_ = QUEUED;
00700 return;
00701 case actionlib:: CommState::ACTIVE:
00702 case actionlib::CommState::PREEMPTING:
00703 controller_status_ = ACTIVE;
00704 return;
00705 case actionlib::CommState::DONE:
00706 {
00707 switch(gh.getTerminalState().state_)
00708 {
00709 case actionlib::TerminalState::RECALLED:
00710 case actionlib::TerminalState::REJECTED:
00711 case actionlib::TerminalState::PREEMPTED:
00712 case actionlib::TerminalState::ABORTED:
00713 case actionlib::TerminalState::LOST:
00714 {
00715 ROS_INFO("Trajectory controller status came back as failed");
00716 controller_status_ = FAILED;
00717 controller_goal_handle_.reset();
00718 return;
00719 }
00720 case actionlib::TerminalState::SUCCEEDED:
00721 {
00722 controller_goal_handle_.reset();
00723 controller_status_ = SUCCESS;
00724 return;
00725 }
00726 default:
00727 ROS_ERROR("Unknown terminal state [%u]. This is a bug in ActionClient", gh.getTerminalState().state_);
00728 }
00729 }
00730 default:
00731 break;
00732 }
00733 }
00734 bool isControllerDone(arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
00735 {
00736 if (controller_status_ == SUCCESS)
00737 {
00738 error_code.val = error_code.SUCCESS;
00739 return true;
00740 } else if(controller_status_ == FAILED)
00741 {
00742 error_code.val = error_code.TRAJECTORY_CONTROLLER_FAILED;
00743 return true;
00744 } else {
00745 return false;
00746 }
00747 }
00748
00749 void fillTrajectoryMsg(const trajectory_msgs::JointTrajectory &trajectory_in,
00750 trajectory_msgs::JointTrajectory &trajectory_out)
00751 {
00752 trajectory_out = trajectory_in;
00753 if(trajectory_in.points.empty())
00754 {
00755 ROS_WARN("No points in trajectory");
00756 return;
00757 }
00758
00759 double d = 0.0;
00760
00761
00762 std::map<std::string, double> val_map;
00763 resetToStartState(planning_scene_state_);
00764 planning_scene_state_->getKinematicStateValues(val_map);
00765 sensor_msgs::JointState current;
00766 current.name = trajectory_out.joint_names;
00767 current.position.resize(trajectory_out.joint_names.size());
00768 for(unsigned int i = 0; i < trajectory_out.joint_names.size(); i++) {
00769 current.position[i] = val_map[trajectory_out.joint_names[i]];
00770 }
00771 std::map<std::string, bool> continuous;
00772 for(unsigned int j = 0; j < trajectory_in.joint_names.size(); j++) {
00773 std::string name = trajectory_in.joint_names[j];
00774 boost::shared_ptr<const urdf::Joint> joint = collision_models_->getParsedDescription()->getJoint(name);
00775 if (joint.get() == NULL)
00776 {
00777 ROS_ERROR("Joint name %s not found in urdf model", name.c_str());
00778 return;
00779 }
00780 if (joint->type == urdf::Joint::CONTINUOUS) {
00781 continuous[name] = true;
00782 } else {
00783 continuous[name] = false;
00784 }
00785 }
00786 for (unsigned int i = 0 ; i < current.position.size() ; ++i)
00787 {
00788 double diff;
00789 if(!continuous[trajectory_in.joint_names[i]]) {
00790 diff = fabs(trajectory_in.points[0].positions[i] - val_map[trajectory_in.joint_names[i]]);
00791 } else {
00792 diff = angles::shortest_angular_distance(trajectory_in.points[0].positions[i],val_map[trajectory_in.joint_names[i]]);
00793 }
00794 d += diff * diff;
00795 }
00796 d = sqrt(d);
00797
00798 int include_first = (d > 0.1) ? 1 : 0;
00799 double offset = 0.0;
00800 trajectory_out.points.resize(trajectory_in.points.size() + include_first);
00801
00802 if (include_first)
00803 {
00804 ROS_INFO("Adding current state to front of trajectory");
00805
00806
00807
00808
00809
00810
00811
00812
00813
00814
00815
00816
00817
00818
00819
00820 trajectory_out.points[0].positions = arm_navigation_msgs::jointStateToJointTrajectoryPoint(current).positions;
00821 trajectory_out.points[0].time_from_start = ros::Duration(0.0);
00822 offset = 0.3 + d;
00823 }
00824 for (unsigned int i = 0 ; i < trajectory_in.points.size() ; ++i)
00825 {
00826 trajectory_out.points[i+include_first].time_from_start = trajectory_in.points[i].time_from_start;
00827 trajectory_out.points[i+include_first].positions = trajectory_in.points[i].positions;
00828 }
00829 trajectory_out.header.stamp = ros::Time::now();
00830 }
00831
00835
00839 void resetStateMachine()
00840 {
00841 num_planning_attempts_ = 0;
00842 current_trajectory_.points.clear();
00843 current_trajectory_.joint_names.clear();
00844 state_ = PLANNING;
00845 }
00846 bool executeCycle(arm_navigation_msgs::GetMotionPlan::Request &req)
00847 {
00848 arm_navigation_msgs::GetMotionPlan::Response res;
00849 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00850
00851 switch(state_)
00852 {
00853 case PLANNING:
00854 {
00855 move_arm_action_feedback_.state = "planning";
00856 move_arm_action_feedback_.time_to_completion = ros::Duration(req.motion_plan_request.allowed_planning_time);
00857 action_server_->publishFeedback(move_arm_action_feedback_);
00858
00859 if(!doPrePlanningChecks(req,res))
00860 return true;
00861
00862 visualizeJointGoal(req);
00863 resetToStartState(planning_scene_state_);
00864 if(collision_models_->isKinematicStateValid(*planning_scene_state_,
00865 group_joint_names_,
00866 error_code,
00867 original_request_.motion_plan_request.goal_constraints,
00868 original_request_.motion_plan_request.path_constraints,
00869 false)) {
00870 resetStateMachine();
00871 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS;
00872 action_server_->setSucceeded(move_arm_action_result_);
00873 ROS_INFO("Apparently start state satisfies goal");
00874 return true;
00875 }
00876 ros::Time planning_time = ros::Time::now();
00877 if(createPlan(req,res))
00878 {
00879 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes;
00880 move_arm_stats_.planning_time = (ros::Time::now()-planning_time).toSec();
00881 ROS_DEBUG("createPlan succeeded");
00882 resetToStartState(planning_scene_state_);
00883 if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_,
00884 res.trajectory.joint_trajectory,
00885 original_request_.motion_plan_request.goal_constraints,
00886 original_request_.motion_plan_request.path_constraints,
00887 error_code,
00888 traj_error_codes,
00889 true))
00890 {
00891 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00892 ROS_WARN("Planner trajectory collides");
00893 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00894 ROS_WARN("Planner trajectory violates path constraints");
00895 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00896 ROS_WARN("Planner trajectory violates joint limits");
00897 } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00898 ROS_WARN("Planner trajectory doesn't reach goal");
00899 }
00900 num_planning_attempts_++;
00901 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00902 {
00903 resetStateMachine();
00904 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00905 action_server_->setAborted(move_arm_action_result_);
00906 return true;
00907 }
00908 }
00909 else{
00910 ROS_DEBUG("Trajectory validity check was successful");
00911
00912 current_trajectory_ = res.trajectory.joint_trajectory;
00913 visualizePlan(current_trajectory_);
00914
00915 state_ = START_CONTROL;
00916 ROS_DEBUG("Done planning. Transitioning to control");
00917 }
00918 }
00919 else if(action_server_->isActive())
00920 {
00921 num_planning_attempts_++;
00922 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00923 error_code.val = error_code.PLANNING_FAILED;
00924 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00925 {
00926 resetStateMachine();
00927 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00928 action_server_->setAborted(move_arm_action_result_);
00929 return true;
00930 }
00931 }
00932 else
00933 {
00934 ROS_ERROR("create plan failed");
00935 }
00936 break;
00937 }
00938 case START_CONTROL:
00939 {
00940 move_arm_action_feedback_.state = "start_control";
00941 move_arm_action_feedback_.time_to_completion = ros::Duration(1.0/move_arm_frequency_);
00942 action_server_->publishFeedback(move_arm_action_feedback_);
00943 ROS_DEBUG("Filtering Trajectory");
00944 trajectory_msgs::JointTrajectory filtered_trajectory;
00945 if(filterTrajectory(current_trajectory_, filtered_trajectory))
00946 {
00947 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00948 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_error_codes;
00949 resetToStartState(planning_scene_state_);
00950 if(!collision_models_->isJointTrajectoryValid(*planning_scene_state_,
00951 filtered_trajectory,
00952 original_request_.motion_plan_request.goal_constraints,
00953 original_request_.motion_plan_request.path_constraints,
00954 error_code,
00955 traj_error_codes,
00956 false))
00957 {
00958 if(error_code.val == error_code.COLLISION_CONSTRAINTS_VIOLATED) {
00959 ROS_WARN("Filtered trajectory collides");
00960 } else if (error_code.val == error_code.PATH_CONSTRAINTS_VIOLATED) {
00961 ROS_WARN("Filtered trajectory violates path constraints");
00962 } else if (error_code.val == error_code.JOINT_LIMITS_VIOLATED) {
00963 ROS_WARN("Filtered trajectory violates joint limits");
00964 } else if (error_code.val == error_code.GOAL_CONSTRAINTS_VIOLATED) {
00965 ROS_WARN("Filtered trajectory doesn't reach goal");
00966 }
00967 ROS_ERROR("Move arm will abort this goal. Will replan");
00968 state_ = PLANNING;
00969 num_planning_attempts_++;
00970 if(num_planning_attempts_ > req.motion_plan_request.num_planning_attempts)
00971 {
00972 resetStateMachine();
00973 ROS_INFO_STREAM("Setting aborted because we're out of planning attempts");
00974 action_server_->setAborted(move_arm_action_result_);
00975 return true;
00976 }
00977
00978
00979 break;
00980
00981 }
00982 else{
00983 ROS_DEBUG("Trajectory validity check was successful");
00984 }
00985 current_trajectory_ = filtered_trajectory;
00986 } else {
00987 resetStateMachine();
00988 ROS_INFO_STREAM("Setting aborted because trajectory filter call failed");
00989 action_server_->setAborted(move_arm_action_result_);
00990 return true;
00991 }
00992 ROS_DEBUG("Sending trajectory");
00993 move_arm_stats_.time_to_execution = (ros::Time::now() - ros::Time(move_arm_stats_.time_to_execution)).toSec();
00994 if(sendTrajectory(current_trajectory_))
00995 {
00996 state_ = MONITOR;
00997 }
00998 else
00999 {
01000 resetStateMachine();
01001 ROS_INFO("Setting aborted because we couldn't send the trajectory");
01002 action_server_->setAborted(move_arm_action_result_);
01003 return true;
01004 }
01005 break;
01006 }
01007 case MONITOR:
01008 {
01009 move_arm_action_feedback_.state = "monitor";
01010 move_arm_action_feedback_.time_to_completion = current_trajectory_.points.back().time_from_start;
01011 action_server_->publishFeedback(move_arm_action_feedback_);
01012 ROS_DEBUG("Start to monitor");
01013 arm_navigation_msgs::ArmNavigationErrorCodes controller_error_code;
01014 if(isControllerDone(controller_error_code))
01015 {
01016 move_arm_stats_.time_to_result = (ros::Time::now()-ros::Time(move_arm_stats_.time_to_result)).toSec();
01017
01018 arm_navigation_msgs::RobotState empty_state;
01019 arm_navigation_msgs::ArmNavigationErrorCodes state_error_code;
01020 getRobotState(planning_scene_state_);
01021 if(collision_models_->isKinematicStateValid(*planning_scene_state_,
01022 group_joint_names_,
01023 state_error_code,
01024 original_request_.motion_plan_request.goal_constraints,
01025 original_request_.motion_plan_request.path_constraints,
01026 true))
01027 {
01028 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.SUCCESS;
01029 resetStateMachine();
01030 action_server_->setSucceeded(move_arm_action_result_);
01031 if(controller_error_code.val == controller_error_code.TRAJECTORY_CONTROLLER_FAILED) {
01032 ROS_INFO("Trajectory controller failed but we seem to be at goal");
01033 } else {
01034 ROS_DEBUG("Reached goal");
01035 }
01036 return true;
01037 }
01038 else
01039 {
01040 if(state_error_code.val == state_error_code.COLLISION_CONSTRAINTS_VIOLATED) {
01041 move_arm_action_result_.error_code.val = state_error_code.START_STATE_IN_COLLISION;
01042 collision_models_->getAllCollisionsForState(*planning_scene_state_,
01043 move_arm_action_result_.contacts);
01044 ROS_WARN("Though trajectory is done current state is in collision");
01045 } else if (state_error_code.val == state_error_code.PATH_CONSTRAINTS_VIOLATED) {
01046 ROS_WARN("Though trajectory is done current state violates path constraints");
01047 } else if (state_error_code.val == state_error_code.JOINT_LIMITS_VIOLATED) {
01048 ROS_WARN("Though trajectory is done current state violates joint limits");
01049 } else if(state_error_code.val == state_error_code.GOAL_CONSTRAINTS_VIOLATED) {
01050 ROS_WARN("Though trajectory is done current state does not seem to be at goal");
01051 }
01052 resetStateMachine();
01053 action_server_->setAborted(move_arm_action_result_);
01054 ROS_INFO("Monitor done but not in good state");
01055 return true;
01056 }
01057 }
01058 break;
01059 }
01060 default:
01061 {
01062 ROS_INFO("Should not be here.");
01063 break;
01064 }
01065 }
01066 if(!action_server_->isActive())
01067 {
01068 ROS_DEBUG("Move arm no longer has an active goal");
01069 return true;
01070 }
01071 return false;
01072 }
01073 void execute(const arm_navigation_msgs::MoveArmGoalConstPtr& goal)
01074 {
01075 arm_navigation_msgs::GetMotionPlan::Request req;
01076 moveArmGoalToPlannerRequest(goal,req);
01077
01078 if(!getAndSetPlanningScene(goal->planning_scene_diff, goal->operations)) {
01079 ROS_INFO("Problem setting planning scene");
01080 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE;
01081 action_server_->setAborted(move_arm_action_result_);
01082 return;
01083 }
01084
01085 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01086 req.motion_plan_request.goal_constraints);
01087
01088 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01089 req.motion_plan_request.path_constraints);
01090
01091
01092 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01093 ros::Time::now(),
01094 collision_models_->getWorldFrameId(),
01095 req.motion_plan_request.start_state);
01096 original_request_ = req;
01097
01098 ros::Rate move_arm_rate(move_arm_frequency_);
01099 move_arm_action_result_.contacts.clear();
01100 move_arm_action_result_.error_code.val = 0;
01101 move_arm_stats_.time_to_execution = ros::Time::now().toSec();
01102 move_arm_stats_.time_to_result = ros::Time::now().toSec();
01103 while(private_handle_.ok())
01104 {
01105 if (action_server_->isPreemptRequested())
01106 {
01107 revertPlanningScene();
01108 move_arm_stats_.preempted = true;
01109 if(publish_stats_)
01110 publishStats();
01111 move_arm_stats_.time_to_execution = ros::Time::now().toSec();
01112 move_arm_stats_.time_to_result = ros::Time::now().toSec();
01113 if(action_server_->isNewGoalAvailable())
01114 {
01115 move_arm_action_result_.contacts.clear();
01116 move_arm_action_result_.error_code.val = 0;
01117 const arm_navigation_msgs::MoveArmGoalConstPtr& new_goal = action_server_->acceptNewGoal();
01118 moveArmGoalToPlannerRequest(new_goal,req);
01119 ROS_DEBUG("Received new goal, will preempt previous goal");
01120 if(!getAndSetPlanningScene(new_goal->planning_scene_diff, new_goal->operations)) {
01121 ROS_INFO("Problem setting planning scene");
01122 move_arm_action_result_.error_code.val = move_arm_action_result_.error_code.INCOMPLETE_ROBOT_STATE;
01123 action_server_->setAborted(move_arm_action_result_);
01124 return;
01125 }
01126
01127 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01128 req.motion_plan_request.goal_constraints);
01129
01130 collision_models_->convertConstraintsGivenNewWorldTransform(*planning_scene_state_,
01131 req.motion_plan_request.path_constraints);
01132 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01133 ros::Time::now(),
01134 collision_models_->getWorldFrameId(),
01135 req.motion_plan_request.start_state);
01136 original_request_ = req;
01137
01138 state_ = PLANNING;
01139 }
01140 else
01141 {
01142 ROS_DEBUG("The move arm action was preempted by the action client. Preempting this goal.");
01143 revertPlanningScene();
01144 resetStateMachine();
01145 action_server_->setPreempted();
01146 return;
01147 }
01148 }
01149
01150
01151 ros::WallTime start = ros::WallTime::now();
01152
01153
01154 bool done = executeCycle(req);
01155
01156
01157 if(done)
01158 {
01159 if(publish_stats_)
01160 publishStats();
01161 return;
01162 }
01163
01164 ros::WallDuration t_diff = ros::WallTime::now() - start;
01165 ROS_DEBUG("Full control cycle time: %.9f\n", t_diff.toSec());
01166
01167 move_arm_rate.sleep();
01168 }
01169
01170 ROS_INFO("Node was killed, aborting");
01171 action_server_->setAborted(move_arm_action_result_);
01172 }
01173
01174 bool getAndSetPlanningScene(const arm_navigation_msgs::PlanningScene& planning_diff,
01175 const arm_navigation_msgs::OrderedCollisionOperations& operations) {
01176 arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
01177 arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
01178
01179 revertPlanningScene();
01180
01181 planning_scene_req.planning_scene_diff = planning_diff;
01182 planning_scene_req.operations = operations;
01183
01184 if(!set_planning_scene_diff_client_.call(planning_scene_req, planning_scene_res)) {
01185 ROS_WARN("Can't get planning scene");
01186 return false;
01187 }
01188
01189 current_planning_scene_ = planning_scene_res.planning_scene;
01190
01191 planning_scene_state_ = collision_models_->setPlanningScene(current_planning_scene_);
01192
01193 collision_models_->disableCollisionsForNonUpdatedLinks(group_);
01194
01195 if(planning_scene_state_ == NULL) {
01196 ROS_WARN("Problems setting local state");
01197 return false;
01198 }
01199 return true;
01200 }
01201
01202 void resetToStartState(planning_models::KinematicState* state) {
01203 planning_environment::setRobotStateAndComputeTransforms(current_planning_scene_.robot_state, *state);
01204 }
01205
01206 bool revertPlanningScene() {
01207 if(planning_scene_state_ != NULL) {
01208 collision_models_->revertPlanningScene(planning_scene_state_);
01209 planning_scene_state_ = NULL;
01210 }
01211 return true;
01212 }
01213
01217
01221 void publishStats()
01222 {
01223 move_arm_stats_.error_code.val = move_arm_action_result_.error_code.val;
01224 move_arm_stats_.result = arm_navigation_msgs::armNavigationErrorCodeToString(move_arm_action_result_.error_code);
01225 stats_publisher_.publish(move_arm_stats_);
01226
01227 move_arm_stats_.error_code.val = 0;
01228 move_arm_stats_.result = " ";
01229 move_arm_stats_.request_id++;
01230 move_arm_stats_.planning_time = -1.0;
01231 move_arm_stats_.smoothing_time = -1.0;
01232 move_arm_stats_.ik_time = -1.0;
01233 move_arm_stats_.time_to_execution = -1.0;
01234 move_arm_stats_.time_to_result = -1.0;
01235 move_arm_stats_.preempted = false;
01236 move_arm_stats_.num_replans = 0.0;
01237 move_arm_stats_.trajectory_duration = -1.0;
01238 }
01239
01240 void printTrajectory(const trajectory_msgs::JointTrajectory &trajectory)
01241 {
01242 for (unsigned int i = 0 ; i < trajectory.points.size() ; ++i)
01243 {
01244 std::stringstream ss;
01245 for (unsigned int j = 0 ; j < trajectory.points[i].positions.size() ; ++j)
01246 ss << trajectory.points[i].positions[j] << " ";
01247 ss << trajectory.points[i].time_from_start.toSec();
01248 ROS_DEBUG("%s", ss.str().c_str());
01249 }
01250 }
01251 void visualizeJointGoal(arm_navigation_msgs::GetMotionPlan::Request &req)
01252 {
01253
01254
01255
01256
01257
01258 ROS_DEBUG("Displaying joint goal");
01259 arm_navigation_msgs::DisplayTrajectory d_path;
01260 d_path.model_id = req.motion_plan_request.group_name;
01261 d_path.trajectory.joint_trajectory = arm_navigation_msgs::jointConstraintsToJointTrajectory(req.motion_plan_request.goal_constraints.joint_constraints);
01262 resetToStartState(planning_scene_state_);
01263 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01264 ros::Time::now(),
01265 collision_models_->getWorldFrameId(),
01266 d_path.robot_state);
01267 display_joint_goal_publisher_.publish(d_path);
01268 ROS_DEBUG("Displaying move arm joint goal.");
01269 }
01270 void visualizeJointGoal(const trajectory_msgs::JointTrajectory &trajectory)
01271 {
01272 ROS_DEBUG("Displaying joint goal");
01273 arm_navigation_msgs::DisplayTrajectory d_path;
01274 d_path.trajectory.joint_trajectory = trajectory;
01275 resetToStartState(planning_scene_state_);
01276 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01277 ros::Time::now(),
01278 collision_models_->getWorldFrameId(),
01279 d_path.robot_state);
01280 display_joint_goal_publisher_.publish(d_path);
01281 }
01282 void visualizePlan(const trajectory_msgs::JointTrajectory &trajectory)
01283 {
01284 move_arm_action_feedback_.state = "visualizing plan";
01285 if(action_server_->isActive())
01286 action_server_->publishFeedback(move_arm_action_feedback_);
01287 arm_navigation_msgs::DisplayTrajectory d_path;
01288 d_path.model_id = original_request_.motion_plan_request.group_name;
01289 d_path.trajectory.joint_trajectory = trajectory;
01290 resetToStartState(planning_scene_state_);
01291 planning_environment::convertKinematicStateToRobotState(*planning_scene_state_,
01292 ros::Time::now(),
01293 collision_models_->getWorldFrameId(),
01294 d_path.robot_state);
01295 display_path_publisher_.publish(d_path);
01296 }
01297 void visualizeAllowedContactRegions(const std::vector<arm_navigation_msgs::AllowedContactSpecification> &allowed_contacts)
01298 {
01299 static int count = 0;
01300 visualization_msgs::MarkerArray mk;
01301 mk.markers.resize(allowed_contacts.size());
01302 for(unsigned int i=0; i < allowed_contacts.size(); i++)
01303 {
01304 bool valid_shape = true;
01305 mk.markers[i].header.stamp = ros::Time::now();
01306 mk.markers[i].header.frame_id = allowed_contacts[i].pose_stamped.header.frame_id;
01307 mk.markers[i].ns = "move_arm::"+allowed_contacts[i].name;
01308 mk.markers[i].id = count++;
01309 if(allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::SPHERE)
01310 {
01311 mk.markers[i].type = visualization_msgs::Marker::SPHERE;
01312 if(allowed_contacts[i].shape.dimensions.size() >= 1)
01313 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[0];
01314 else
01315 valid_shape = false;
01316 }
01317 else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::BOX)
01318 {
01319 mk.markers[i].type = visualization_msgs::Marker::CUBE;
01320 if(allowed_contacts[i].shape.dimensions.size() >= 3)
01321 {
01322 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
01323 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[1];
01324 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[2];
01325 }
01326 else
01327 valid_shape = false;
01328 }
01329 else if (allowed_contacts[i].shape.type == arm_navigation_msgs::Shape::CYLINDER)
01330 {
01331 mk.markers[i].type = visualization_msgs::Marker::CYLINDER;
01332 if(allowed_contacts[i].shape.dimensions.size() >= 2)
01333 {
01334 mk.markers[i].scale.x = allowed_contacts[i].shape.dimensions[0];
01335 mk.markers[i].scale.y = allowed_contacts[i].shape.dimensions[0];
01336 mk.markers[i].scale.z = allowed_contacts[i].shape.dimensions[1];
01337 }
01338 else
01339 valid_shape = false;
01340 }
01341 else
01342 {
01343 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
01344 valid_shape = false;
01345 }
01346
01347 mk.markers[i].action = visualization_msgs::Marker::ADD;
01348 mk.markers[i].pose = allowed_contacts[i].pose_stamped.pose;
01349 if(!valid_shape)
01350 {
01351 mk.markers[i].scale.x = mk.markers[i].scale.y = mk.markers[i].scale.z = 0.01;
01352 mk.markers[i].color.a = 0.3;
01353 mk.markers[i].color.r = 1.0;
01354 mk.markers[i].color.g = 0.04;
01355 mk.markers[i].color.b = 0.04;
01356 }
01357 else
01358 {
01359 mk.markers[i].color.a = 1.0;
01360 mk.markers[i].color.r = 0.04;
01361 mk.markers[i].color.g = 1.0;
01362 mk.markers[i].color.b = 0.04;
01363 }
01364
01365 }
01366 allowed_contact_regions_publisher_.publish(mk);
01367 }
01371
01372 private:
01373
01374 std::string group_;
01375
01376 ros::ServiceClient ik_client_;
01377 ros::ServiceClient trajectory_start_client_,trajectory_cancel_client_,trajectory_query_client_;
01378 ros::NodeHandle private_handle_, root_handle_;
01379 boost::shared_ptr<actionlib::SimpleActionServer<arm_navigation_msgs::MoveArmAction> > action_server_;
01380
01381 planning_environment::CollisionModels* collision_models_;
01382
01383 arm_navigation_msgs::SetPlanningSceneDiff::Request set_planning_scene_diff_req_;
01384 arm_navigation_msgs::SetPlanningSceneDiff::Response set_planning_scene_diff_res_;
01385 arm_navigation_msgs::PlanningScene current_planning_scene_;
01386 planning_models::KinematicState* planning_scene_state_;
01387
01388 tf::TransformListener *tf_;
01389 MoveArmState state_;
01390 double move_arm_frequency_;
01391 trajectory_msgs::JointTrajectory current_trajectory_;
01392
01393 int num_planning_attempts_;
01394
01395 std::vector<std::string> group_joint_names_;
01396 std::vector<std::string> group_link_names_;
01397 std::vector<std::string> all_link_names_;
01398 arm_navigation_msgs::MoveArmResult move_arm_action_result_;
01399 arm_navigation_msgs::MoveArmFeedback move_arm_action_feedback_;
01400
01401 arm_navigation_msgs::GetMotionPlan::Request original_request_;
01402
01403 ros::Publisher display_path_publisher_;
01404 ros::Publisher display_joint_goal_publisher_;
01405 ros::Publisher allowed_contact_regions_publisher_;
01406 ros::Publisher vis_marker_publisher_;
01407 ros::Publisher vis_marker_array_publisher_;
01408 ros::ServiceClient filter_trajectory_client_;
01409 ros::ServiceClient get_state_client_;
01410 ros::ServiceClient set_planning_scene_diff_client_;
01411 MoveArmParameters move_arm_parameters_;
01412
01413 ControllerStatus controller_status_;
01414
01415 JointExecutorActionClient* controller_action_client_;
01416 JointExecutorActionClient::GoalHandle controller_goal_handle_;
01417
01418 double trajectory_filter_allowed_time_, ik_allowed_time_;
01419 double trajectory_discretization_;
01420 bool arm_ik_initialized_;
01421
01422 bool publish_stats_;
01423 arm_navigation_msgs::MoveArmStatistics move_arm_stats_;
01424 ros::Publisher stats_publisher_;
01425
01426 };
01427 }
01428
01429 int main(int argc, char** argv)
01430 {
01431 ros::init(argc, argv, "move_arm");
01432
01433 ros::AsyncSpinner spinner(1);
01434 spinner.start();
01435 ros::NodeHandle nh("~");
01436 std::string group;
01437 nh.param<std::string>("group", group, std::string());
01438 ROS_INFO("Move arm operating on group %s",group.c_str());
01439 move_arm::MoveArm move_arm(group);
01440 if(!move_arm.configure())
01441 {
01442 ROS_ERROR("Could not configure move arm, exiting");
01443 ros::shutdown();
01444 return 1;
01445 }
01446 ROS_INFO("Move arm action started");
01447 ros::waitForShutdown();
01448
01449 return 0;
01450 }