$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2010, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * \author Adam Harmat, E. Gil Jones 00036 *********************************************************************/ 00037 00038 #include <ros/ros.h> 00039 00040 #include <planning_environment/models/collision_models_interface.h> 00041 #include <planning_environment/monitors/kinematic_model_state_monitor.h> 00042 #include <planning_environment/models/model_utils.h> 00043 #include <planning_environment/monitors/monitor_utils.h> 00044 #include <arm_navigation_msgs/convert_messages.h> 00045 #include "planning_environment/util/construct_object.h" 00046 00047 #include <sensor_msgs/PointCloud2.h> 00048 #include <pcl/point_types.h> 00049 #include <pcl/point_cloud.h> 00050 #include <pcl/ros/conversions.h> 00051 #include <pcl_ros/transforms.h> 00052 00053 #include <actionlib/server/simple_action_server.h> 00054 #include <actionlib/client/simple_action_client.h> 00055 #include <actionlib/client/simple_client_goal_state.h> 00056 00057 #include <pr2_controllers_msgs/PointHeadAction.h> 00058 #include <control_msgs/FollowJointTrajectoryAction.h> 00059 00060 #include <head_monitor_msgs/HeadMonitorStatus.h> 00061 #include <head_monitor_msgs/HeadMonitorAction.h> 00062 #include <head_monitor_msgs/PreplanHeadScanAction.h> 00063 00064 #include <visualization_msgs/MarkerArray.h> 00065 #include <visualization_msgs/Marker.h> 00066 00067 #include <tf/transform_listener.h> 00068 00069 #include <boost/thread/mutex.hpp> 00070 00071 static const std::string RIGHT_ARM_GROUP = "right_arm"; 00072 static const std::string LEFT_ARM_GROUP = "left_arm"; 00073 00074 static const double JOINT_BOUNDS_MARGIN = .02; 00075 00076 std::string convertFromGroupNameToArmName(const std::string& arm_name) { 00077 if(arm_name.find(RIGHT_ARM_GROUP) != std::string::npos) { 00078 return(RIGHT_ARM_GROUP); 00079 } else if(arm_name.find(LEFT_ARM_GROUP) != std::string::npos) { 00080 return(LEFT_ARM_GROUP); 00081 } else { 00082 ROS_WARN_STREAM("Neither left arm group or right arm group in group: " << arm_name); 00083 } 00084 return std::string(""); 00085 } 00086 00087 class HeadMonitor 00088 { 00089 protected: 00090 00091 ros::NodeHandle private_handle_; 00092 ros::NodeHandle root_handle_; 00093 actionlib::SimpleActionServer<head_monitor_msgs::HeadMonitorAction> head_monitor_action_server_; 00094 actionlib::SimpleActionServer<head_monitor_msgs::PreplanHeadScanAction> head_preplan_scan_action_server_; 00095 00096 ros::Publisher marker_pub_; 00097 00098 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> head_controller_action_client_; 00099 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>* right_arm_controller_action_client_; 00100 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>* left_arm_controller_action_client_; 00101 actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> point_head_action_client_; 00102 00103 std::string current_group_name_; 00104 actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>* current_arm_controller_action_client_; 00105 00106 bool use_left_arm_; 00107 bool use_right_arm_; 00108 00109 planning_environment::CollisionModelsInterface* collision_models_interface_; 00110 planning_environment::KinematicModelStateMonitor* kmsm_; 00111 00112 head_monitor_msgs::HeadMonitorGoal monitor_goal_; 00113 head_monitor_msgs::HeadMonitorFeedback monitor_feedback_; 00114 head_monitor_msgs::HeadMonitorResult monitor_result_; 00115 00116 sensor_msgs::JointState last_joint_state_; 00117 00118 trajectory_msgs::JointTrajectory logged_trajectory_; 00119 ros::Time logged_trajectory_start_time_; 00120 00121 tf::TransformListener tf_; 00122 00123 ros::Timer paused_callback_timer_; 00124 ros::Timer start_trajectory_timer_; 00125 00126 visualization_msgs::Marker marker_; 00127 00128 tf::MessageFilter<sensor_msgs::PointCloud2> *mn_; 00129 message_filters::Subscriber<sensor_msgs::PointCloud2> *sub_; 00130 00131 //TODO - get rid of once we have Adam's stuff 00132 double start_head_pan_, start_head_tilt_, goal_head_pan_, goal_head_tilt_; 00133 00134 double point_sphere_size_; 00135 double pause_time_; 00136 double max_point_distance_; 00137 00138 bool do_monitoring_; 00139 bool do_preplan_scan_; 00140 00141 head_monitor_msgs::HeadMonitorStatus current_execution_status_; 00142 00143 boost::mutex mutex_; 00144 00145 public: 00146 00147 HeadMonitor() : 00148 private_handle_("~"), 00149 head_monitor_action_server_(root_handle_, "head_monitor_action", false), 00150 head_preplan_scan_action_server_(root_handle_, "preplan_head_scan", boost::bind(&HeadMonitor::preplanHeadScanCallback, this, _1), false), 00151 head_controller_action_client_("/head_traj_controller/follow_joint_trajectory", true), 00152 point_head_action_client_("/head_traj_controller/point_head_action", true) 00153 { 00154 current_execution_status_.status = current_execution_status_.IDLE; 00155 00156 private_handle_.param<double>("point_sphere_size", point_sphere_size_, .01); 00157 private_handle_.param<double>("pause_time", pause_time_, 5.0); 00158 private_handle_.param<double>("max_point_distance", max_point_distance_, 1.0); 00159 private_handle_.param<bool>("do_preplan_scan", do_preplan_scan_, true); 00160 private_handle_.param<bool>("do_monitoring", do_monitoring_, true); 00161 private_handle_.param<bool>("use_left_arm", use_left_arm_, true); 00162 private_handle_.param<bool>("use_right_arm", use_right_arm_, true); 00163 00164 std::string robot_description_name = root_handle_.resolveName("robot_description", true); 00165 00166 collision_models_interface_ = new planning_environment::CollisionModelsInterface(robot_description_name); 00167 kmsm_ = new planning_environment::KinematicModelStateMonitor(collision_models_interface_, &tf_); 00168 00169 kmsm_->addOnStateUpdateCallback(boost::bind(&HeadMonitor::jointStateCallback, this, _1)); 00170 00171 if(do_preplan_scan_ && do_monitoring_) { 00172 sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud2> (root_handle_, "cloud_in", 1); 00173 mn_ = new tf::MessageFilter<sensor_msgs::PointCloud2> (*sub_, tf_, "", 1); 00174 mn_->setTargetFrame(collision_models_interface_->getWorldFrameId()); 00175 mn_->registerCallback(boost::bind(&HeadMonitor::cloudCallback, this, _1)); 00176 } 00177 00178 head_monitor_action_server_.registerGoalCallback(boost::bind(&HeadMonitor::monitorGoalCallback, this)); 00179 head_monitor_action_server_.registerPreemptCallback(boost::bind(&HeadMonitor::monitorPreemptCallback, this)); 00180 00181 if(use_right_arm_) { 00182 right_arm_controller_action_client_ = new actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>("/r_arm_controller/follow_joint_trajectory", true); 00183 while(ros::ok() && !right_arm_controller_action_client_->waitForServer(ros::Duration(1.0))){ 00184 ROS_INFO("Waiting for the right joint_trajectory_action server to come up."); 00185 } 00186 } 00187 if(use_left_arm_) { 00188 left_arm_controller_action_client_ = new actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>("/l_arm_controller/follow_joint_trajectory", true); 00189 while(ros::ok() && !left_arm_controller_action_client_->waitForServer(ros::Duration(1.0))){ 00190 ROS_INFO("Waiting for the left joint_trajectory_action server to come up."); 00191 } 00192 } 00193 00194 if(!use_left_arm_ && !use_right_arm_) { 00195 ROS_ERROR_STREAM("No arms specified. Exiting"); 00196 exit(0); 00197 } 00198 00199 ROS_INFO("Connected to the controllers"); 00200 00201 head_monitor_action_server_.start(); 00202 head_preplan_scan_action_server_.start(); 00203 00204 ROS_INFO_STREAM("Starting head monitor action server for "<<ros::this_node::getName()); 00205 } 00206 00207 ~HeadMonitor(void) 00208 { 00209 delete kmsm_; 00210 delete collision_models_interface_; 00211 delete right_arm_controller_action_client_; 00212 delete left_arm_controller_action_client_; 00213 } 00214 00218 00219 void cloudCallback (const sensor_msgs::PointCloud2ConstPtr &cloud2) 00220 { 00221 if(current_execution_status_.status == current_execution_status_.IDLE || 00222 current_execution_status_.status == current_execution_status_.PREPLAN_SCAN) { 00223 return; 00224 } 00225 00226 ros::Time callback_delay = ros::Time::now(); 00227 00228 pcl::PointCloud<pcl::PointXYZ> pcl_cloud, trans_cloud, near_cloud; 00229 pcl::fromROSMsg(*cloud2, pcl_cloud); 00230 00231 if(pcl_cloud.points.size() == 0) { 00232 ROS_WARN_STREAM("No points in cloud"); 00233 return; 00234 } 00235 00236 collision_models_interface_->bodiesLock(); 00237 00238 sensor_msgs::JointState js = last_joint_state_; 00239 trajectory_msgs::JointTrajectory joint_trajectory_subset; 00240 if(current_execution_status_.status == current_execution_status_.EXECUTING) { 00241 planning_environment::removeCompletedTrajectory(collision_models_interface_->getParsedDescription(), 00242 monitor_goal_.joint_trajectory, 00243 js, 00244 joint_trajectory_subset, 00245 false); 00246 } else { 00247 joint_trajectory_subset = monitor_goal_.joint_trajectory; 00248 } 00249 00250 if(joint_trajectory_subset.points.size() == 0) { 00251 //no points left to check 00252 if(current_execution_status_.status == current_execution_status_.PAUSED) { 00253 ROS_ERROR_STREAM("Paused, but closest trajectory point is goal"); 00254 } 00255 collision_models_interface_->bodiesUnlock(); 00256 return; 00257 } 00258 ros::Time n1 = ros::Time::now(); 00259 00260 planning_models::KinematicState state(collision_models_interface_->getKinematicModel()); 00261 kmsm_->setStateValuesFromCurrentValues(state); 00262 00263 planning_environment::updateAttachedObjectBodyPoses(collision_models_interface_, 00264 state, 00265 tf_); 00266 00267 //now in the right frame 00268 00269 if (collision_models_interface_->getWorldFrameId() != pcl_cloud.header.frame_id) { 00270 pcl_ros::transformPointCloud(collision_models_interface_->getWorldFrameId(), pcl_cloud, trans_cloud, tf_); 00271 } else { 00272 trans_cloud = pcl_cloud; 00273 } 00274 00275 btTransform cur_link_pose = state.getLinkState(monitor_goal_.target_link)->getGlobalCollisionBodyTransform(); 00276 near_cloud.header = trans_cloud.header; 00277 for(unsigned int i = 0; i < trans_cloud.points.size(); i++) { 00278 btVector3 pt = btVector3(trans_cloud.points[i].x, trans_cloud.points[i].y, trans_cloud.points[i].z); 00279 double dist = pt.distance(cur_link_pose.getOrigin()); 00280 if(dist <= max_point_distance_) { 00281 near_cloud.push_back(trans_cloud.points[i]); 00282 } 00283 } 00284 00285 std::vector<shapes::Shape*> spheres(near_cloud.points.size());; 00286 std::vector<btTransform> positions(near_cloud.points.size()); 00287 00288 ros::Time n3 = ros::Time::now(); 00289 00290 if(near_cloud.points.size() != 0) { 00291 00292 btQuaternion ident(0.0, 0.0, 0.0, 1.0); 00293 //making spheres from the points 00294 for (unsigned int i = 0 ; i < near_cloud.points.size(); ++i) { 00295 positions[i] = btTransform(ident, 00296 btVector3(near_cloud.points[i].x, near_cloud.points[i].y, near_cloud.points[i].z)); 00297 spheres[i] = new shapes::Sphere(point_sphere_size_); 00298 } 00299 00300 //collisions should have been turned off with everything but this 00301 collision_models_interface_->addStaticObject("point_spheres", 00302 spheres, 00303 positions, 00304 0.0); 00305 00306 //turning off collisions except between point spheres and downstream links 00307 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_interface_->getCollisionSpace()->getCurrentAllowedCollisionMatrix(); 00308 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_interface_->getKinematicModel()->getModelGroup(current_group_name_); 00309 00310 acm.addEntry("point_spheres", true); 00311 acm.changeEntry(true); 00312 acm.changeEntry("point_spheres", joint_model_group->getUpdatedLinkModelNames(), false); 00313 00314 collision_models_interface_->setAlteredAllowedCollisionMatrix(acm); 00315 00316 } else { 00317 //turning off all collisions 00318 collision_space::EnvironmentModel::AllowedCollisionMatrix acm = collision_models_interface_->getCollisionSpace()->getCurrentAllowedCollisionMatrix(); 00319 acm.changeEntry(true); 00320 00321 collision_models_interface_->setAlteredAllowedCollisionMatrix(acm); 00322 } 00323 arm_navigation_msgs::Constraints empty_constraints; 00324 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00325 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes; 00326 00327 ros::Time n2 = ros::Time::now(); 00328 if(!collision_models_interface_->isJointTrajectoryValid(state, 00329 joint_trajectory_subset, 00330 empty_constraints, 00331 empty_constraints, 00332 error_code, 00333 trajectory_error_codes, 00334 false)) { 00335 ROS_INFO_STREAM("Validity check took " << (ros::Time::now()-n2).toSec()); 00336 ROS_INFO_STREAM("Trajectory check took " << (ros::Time::now() - n1).toSec() 00337 << " shape " << (n2-n3).toSec() 00338 << " traj part " << (ros::Time::now() - n2).toSec()); 00339 if(current_execution_status_.status == current_execution_status_.MONITOR_BEFORE_EXECUTION || 00340 current_execution_status_.status == current_execution_status_.EXECUTING) { 00341 ROS_WARN_STREAM("Trajectory judged invalid " << error_code.val << ". Pausing"); 00342 //TODO - stop more gracefully, do something else? 00343 if(current_execution_status_.status == current_execution_status_.MONITOR_BEFORE_EXECUTION) { 00344 start_trajectory_timer_.stop(); 00345 } else { 00346 current_arm_controller_action_client_->cancelGoal(); 00347 ROS_INFO_STREAM("Delay from data timestamp to cancel is " << (ros::Time::now() - cloud2->header.stamp).toSec() 00348 << " delay to receipt is " << (callback_delay - cloud2->header.stamp).toSec()); 00349 00350 } 00351 stopHead(); 00352 current_execution_status_.status = current_execution_status_.PAUSED; 00353 kmsm_->setStateValuesFromCurrentValues(state); 00354 planning_environment::convertKinematicStateToRobotState(state, 00355 ros::Time::now(), 00356 collision_models_interface_->getWorldFrameId(), 00357 monitor_feedback_.current_state); 00358 unsigned int trajectory_point = trajectory_error_codes.size(); 00359 if(trajectory_error_codes.size() > 0) { 00360 trajectory_point--; 00361 } 00362 ROS_INFO_STREAM("Setting trajectory state to point " << trajectory_point << " of " << joint_trajectory_subset.points.size()); 00363 00364 std::map<std::string, double> vals; 00365 for(unsigned int i = 0; i < joint_trajectory_subset.joint_names.size(); i++) { 00366 vals[joint_trajectory_subset.joint_names[i]] = joint_trajectory_subset.points[trajectory_point].positions[i]; 00367 } 00368 state.setKinematicState(vals); 00369 planning_environment::convertKinematicStateToRobotState(state, 00370 ros::Time::now(), 00371 collision_models_interface_->getWorldFrameId(), 00372 monitor_feedback_.paused_trajectory_state); 00373 monitor_feedback_.paused_collision_map.header.frame_id = collision_models_interface_->getWorldFrameId(); 00374 monitor_feedback_.paused_collision_map.header.stamp = cloud2->header.stamp; 00375 monitor_feedback_.paused_collision_map.id = "point_spheres"; 00376 monitor_feedback_.paused_collision_map.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 00377 for (unsigned int j = 0 ; j < spheres.size(); ++j) { 00378 arm_navigation_msgs::Shape obj; 00379 if (planning_environment::constructObjectMsg(spheres[j], obj)) { 00380 geometry_msgs::Pose pose; 00381 tf::poseTFToMsg(positions[j], pose); 00382 monitor_feedback_.paused_collision_map.shapes.push_back(obj); 00383 monitor_feedback_.paused_collision_map.poses.push_back(pose); 00384 } 00385 } 00386 std::vector<arm_navigation_msgs::ContactInformation> contacts; 00387 collision_models_interface_->getAllCollisionsForState(state, contacts, 1); 00388 if(contacts.size() == 0) { 00389 ROS_INFO_STREAM("No contacts for last trajectory state"); 00390 } else { 00391 for(unsigned int i = 0; i < contacts.size(); i++) { 00392 ROS_INFO_STREAM("Contact between " << contacts[i].contact_body_1 << " and " << contacts[i].contact_body_2 00393 << " at point " << contacts[i].position.x << " " << contacts[i].position.y << " " << contacts[i].position.z); 00394 } 00395 00396 } 00397 head_monitor_action_server_.publishFeedback(monitor_feedback_); 00398 paused_callback_timer_ = root_handle_.createTimer(ros::Duration(pause_time_), boost::bind(&HeadMonitor::pauseTimeoutCallback, this), true); 00399 } 00400 //if paused no need to do anything else, except maybe provide additional feedback? 00401 } else { 00402 if(current_execution_status_.status == current_execution_status_.PAUSED) { 00403 paused_callback_timer_.stop(); 00404 00405 ROS_WARN_STREAM("Unpausing"); 00406 00407 current_execution_status_.status = current_execution_status_.MONITOR_BEFORE_EXECUTION; 00408 00409 sensor_msgs::JointState js = last_joint_state_; 00410 trajectory_msgs::JointTrajectory joint_trajectory_subset; 00411 00412 planning_environment::removeCompletedTrajectory(collision_models_interface_->getParsedDescription(), 00413 monitor_goal_.joint_trajectory, 00414 js, 00415 joint_trajectory_subset, 00416 false); 00417 00418 control_msgs::FollowJointTrajectoryGoal goal; 00419 goal.trajectory = generateHeadTrajectory(monitor_goal_.target_link, joint_trajectory_subset); 00420 head_controller_action_client_.sendGoal(goal); 00421 00422 //Setting timer for actually sending trajectory 00423 start_trajectory_timer_ = root_handle_.createTimer(ros::Duration(monitor_goal_.time_offset), boost::bind(&HeadMonitor::trajectoryTimerCallback, this), true); 00424 } 00425 } 00426 if(near_cloud.points.size() != 0) { 00427 collision_models_interface_->deleteStaticObject("point_spheres"); 00428 monitor_feedback_.paused_collision_map.shapes.clear(); 00429 monitor_feedback_.paused_collision_map.poses.clear(); 00430 } 00431 ROS_DEBUG_STREAM("Trajectory check took " << (ros::Time::now() - n1).toSec() 00432 << " shape " << (n2-n3).toSec() 00433 << " traj part " << (ros::Time::now() - n2).toSec()); 00434 collision_models_interface_->bodiesUnlock(); 00435 } 00436 00437 void moveInsideSafetyLimits(const head_monitor_msgs::PreplanHeadScanGoalConstPtr &goal, 00438 const planning_models::KinematicState& state) { 00439 const planning_models::KinematicModel::JointModelGroup* joint_model_group = collision_models_interface_->getKinematicModel()->getModelGroup(goal->group_name); 00440 const std::vector<std::string>& joint_names = joint_model_group->getJointModelNames(); 00441 ROS_DEBUG_STREAM("Group name " << goal->group_name); 00442 00443 if(goal->group_name == RIGHT_ARM_GROUP && !use_right_arm_) { 00444 ROS_WARN_STREAM("Not configured for use with group name " << RIGHT_ARM_GROUP); 00445 return; 00446 } 00447 if(goal->group_name == LEFT_ARM_GROUP && !use_left_arm_) { 00448 ROS_WARN_STREAM("Not configured for use with group name " << LEFT_ARM_GROUP); 00449 return; 00450 } 00451 00452 if(state.areJointsWithinBounds(joint_names)) { 00453 return; 00454 } 00455 trajectory_msgs::JointTrajectory traj; 00456 traj.joint_names = joint_names; 00457 traj.header.stamp = ros::Time::now(); 00458 traj.header.frame_id = collision_models_interface_->getWorldFrameId(); 00459 traj.points.resize(1); 00460 traj.points[0].positions.resize(joint_names.size()); 00461 traj.points[0].velocities.resize(joint_names.size()); 00462 traj.points[0].time_from_start = ros::Duration(.4); 00463 00464 std::map<std::string, double> joint_values; 00465 state.getKinematicStateValues(joint_values); 00466 00467 for(unsigned int j = 0; j < joint_names.size(); j++) { 00468 if(!state.isJointWithinBounds(joint_names[j])) { 00469 std::pair<double, double> bounds; 00470 state.getJointState(joint_names[j])->getJointModel()->getVariableBounds(joint_names[j], bounds); 00471 ROS_INFO_STREAM("Joint " << joint_names[j] << " out of bounds. " << 00472 " value: " << state.getJointState(joint_names[j])->getJointStateValues()[0] << 00473 " low: " << bounds.first << " high: " << bounds.second); 00474 if(joint_values[joint_names[j]] < bounds.first) { 00475 traj.points[0].positions[j] = bounds.first+JOINT_BOUNDS_MARGIN; 00476 ROS_INFO_STREAM("Setting joint " << joint_names[j] << " inside lower bound " << traj.points[0].positions[j]); 00477 } else { 00478 traj.points[0].positions[j] = bounds.second-JOINT_BOUNDS_MARGIN; 00479 ROS_INFO_STREAM("Setting joint " << joint_names[j] << " inside upper bound " << traj.points[0].positions[j]); 00480 } 00481 } else { 00482 traj.points[0].positions[j] = joint_values[joint_names[j]]; 00483 } 00484 } 00485 00486 control_msgs::FollowJointTrajectoryGoal traj_goal; 00487 traj_goal.trajectory = traj; 00488 00489 current_arm_controller_action_client_ = ((goal->group_name == RIGHT_ARM_GROUP) ? right_arm_controller_action_client_ : left_arm_controller_action_client_); 00490 00491 if(current_arm_controller_action_client_->sendGoalAndWait(traj_goal, ros::Duration(1.0), ros::Duration(.5)) != actionlib::SimpleClientGoalState::SUCCEEDED) { 00492 ROS_WARN_STREAM("Joint bounds correction trajectory failed"); 00493 } 00494 } 00495 00496 void preplanHeadScanCallback(const head_monitor_msgs::PreplanHeadScanGoalConstPtr &goal) 00497 { 00498 head_monitor_msgs::PreplanHeadScanResult res; 00499 00500 collision_models_interface_->bodiesLock(); 00501 00502 if(current_execution_status_.status != current_execution_status_.IDLE) { 00503 ROS_WARN_STREAM("Got preplan in something other than IDLE mode"); 00504 } 00505 00506 //need a kinematic state 00507 planning_models::KinematicState state(collision_models_interface_->getKinematicModel()); 00508 00509 kmsm_->setStateValuesFromCurrentValues(state); 00510 00511 moveInsideSafetyLimits(goal, state); 00512 00513 if(!do_preplan_scan_) { 00514 collision_models_interface_->bodiesUnlock(); 00515 head_preplan_scan_action_server_.setSucceeded(res); 00516 return; 00517 } 00518 00519 current_execution_status_.status = current_execution_status_.PREPLAN_SCAN; 00520 00521 kmsm_->setStateValuesFromCurrentValues(state); 00522 00523 //supplementing with start state changes 00524 planning_environment::setRobotStateAndComputeTransforms(goal->motion_plan_request.start_state, 00525 state); 00526 double x_start, y_start, z_start; 00527 00528 if(!state.hasLinkState(goal->head_monitor_link)) { 00529 ROS_WARN_STREAM("No monitor link " << goal->head_monitor_link); 00530 head_preplan_scan_action_server_.setAborted(res); 00531 current_execution_status_.status = current_execution_status_.IDLE; 00532 collision_models_interface_->bodiesUnlock(); 00533 return; 00534 } 00535 00536 btTransform link_state = state.getLinkState(goal->head_monitor_link)->getGlobalCollisionBodyTransform(); 00537 x_start = link_state.getOrigin().x(); 00538 y_start = link_state.getOrigin().y(); 00539 z_start = link_state.getOrigin().z(); 00540 00541 double x_goal, y_goal, z_goal; 00542 if(goal->motion_plan_request.goal_constraints.position_constraints.size() >= 1 00543 && goal->motion_plan_request.goal_constraints.orientation_constraints.size() >= 1) { 00544 geometry_msgs::PoseStamped goal_pose 00545 = arm_navigation_msgs::poseConstraintsToPoseStamped(goal->motion_plan_request.goal_constraints.position_constraints[0], 00546 goal->motion_plan_request.goal_constraints.orientation_constraints[0]); 00547 00548 std::string es; 00549 if (tf_.getLatestCommonTime(collision_models_interface_->getWorldFrameId(), goal_pose.header.frame_id, goal_pose.header.stamp, &es) != tf::NO_ERROR) { 00550 } 00551 geometry_msgs::PoseStamped psout; 00552 tf_.transformPose(collision_models_interface_->getWorldFrameId(), goal_pose, psout); 00553 00554 x_goal = psout.pose.position.x; 00555 y_goal = psout.pose.position.y; 00556 z_goal = psout.pose.position.z; 00557 } else { 00558 if(goal->motion_plan_request.goal_constraints.joint_constraints.size() <= 1) { 00559 ROS_WARN("Not a pose goal and not enough joint constraints"); 00560 head_preplan_scan_action_server_.setAborted(res); 00561 current_execution_status_.status = current_execution_status_.IDLE; 00562 collision_models_interface_->bodiesUnlock(); 00563 return; 00564 } 00565 std::map<std::string, double> joint_values; 00566 for(unsigned int i = 0; i < goal->motion_plan_request.goal_constraints.joint_constraints.size(); i++) { 00567 joint_values[goal->motion_plan_request.goal_constraints.joint_constraints[i].joint_name] = goal->motion_plan_request.goal_constraints.joint_constraints[i].position; 00568 } 00569 state.setKinematicState(joint_values); 00570 btTransform link_state = state.getLinkState(goal->head_monitor_link)->getGlobalCollisionBodyTransform(); 00571 x_goal = link_state.getOrigin().x(); 00572 y_goal = link_state.getOrigin().y(); 00573 z_goal = link_state.getOrigin().z(); 00574 } 00575 std::map<std::string, double> state_values; 00576 ROS_INFO_STREAM("Looking at goal " << x_goal << " " << y_goal << " " << z_goal); 00577 lookAt(collision_models_interface_->getWorldFrameId(), x_goal, y_goal, z_goal, true); 00578 kmsm_->setStateValuesFromCurrentValues(state); 00579 state.getKinematicStateValues(state_values); 00580 goal_head_pan_ = state_values["head_pan_joint"]; 00581 goal_head_tilt_ = state_values["head_tilt_joint"]; 00582 ROS_INFO_STREAM("Looking at start " << x_start << " " << y_start << " " << z_start); 00583 lookAt(collision_models_interface_->getWorldFrameId(), x_start, y_start, z_start, true); 00584 kmsm_->setStateValuesFromCurrentValues(state); 00585 state.getKinematicStateValues(state_values); 00586 start_head_pan_ = state_values["head_pan_joint"]; 00587 start_head_tilt_ = state_values["head_tilt_joint"]; 00588 head_preplan_scan_action_server_.setSucceeded(res); 00589 current_execution_status_.status = current_execution_status_.IDLE; 00590 collision_models_interface_->bodiesUnlock(); 00591 } 00592 00593 // Called when a new monitoring goal is received 00594 void monitorGoalCallback() 00595 { 00596 mutex_.lock(); 00597 ROS_DEBUG("In goal callback for monitor"); 00598 if(current_execution_status_.status == current_execution_status_.MONITOR_BEFORE_EXECUTION && 00599 current_execution_status_.status == current_execution_status_.EXECUTING && 00600 current_execution_status_.status == current_execution_status_.PAUSED) { 00601 ROS_WARN_STREAM("Got new goal while executing"); 00602 mutex_.unlock(); 00603 stopEverything(); 00604 mutex_.lock(); 00605 } 00606 00607 if(!head_monitor_action_server_.isNewGoalAvailable()) 00608 { 00609 ROS_INFO_STREAM("Preempted, no new goal"); 00610 mutex_.unlock(); 00611 return; 00612 } 00613 monitor_goal_ = head_monitor_msgs::HeadMonitorGoal(*head_monitor_action_server_.acceptNewGoal()); 00614 current_group_name_ = convertFromGroupNameToArmName(monitor_goal_.group_name); 00615 00616 if(current_group_name_ == RIGHT_ARM_GROUP && !use_right_arm_) { 00617 ROS_WARN_STREAM("Not configured for use with group name " << RIGHT_ARM_GROUP); 00618 monitor_result_.error_code.val = monitor_result_.error_code.INVALID_GROUP_NAME; 00619 head_monitor_action_server_.setAborted(monitor_result_); 00620 mutex_.unlock(); 00621 return; 00622 } 00623 if(current_group_name_ == LEFT_ARM_GROUP && !use_left_arm_) { 00624 ROS_WARN_STREAM("Not configured for use with group name " << LEFT_ARM_GROUP); 00625 monitor_result_.error_code.val = monitor_result_.error_code.INVALID_GROUP_NAME; 00626 head_monitor_action_server_.setAborted(monitor_result_); 00627 mutex_.unlock(); 00628 return; 00629 } 00630 00631 if(current_group_name_.empty()) { 00632 ROS_WARN_STREAM("Group name doesn't have left or right arm in it, gonna be bad"); 00633 } 00634 mutex_.unlock(); 00635 00636 current_arm_controller_action_client_ = ((current_group_name_ == RIGHT_ARM_GROUP) ? right_arm_controller_action_client_ : left_arm_controller_action_client_); 00637 00638 logged_trajectory_ = monitor_goal_.joint_trajectory; 00639 logged_trajectory_.points.clear(); 00640 00641 logged_trajectory_start_time_ = ros::Time::now(); 00642 00643 if(do_preplan_scan_ && do_monitoring_) { 00644 current_execution_status_.status = current_execution_status_.MONITOR_BEFORE_EXECUTION; 00645 00646 //Generating head trajectory and deploying it 00647 00648 control_msgs::FollowJointTrajectoryGoal goal; 00649 goal.trajectory = generateHeadTrajectory(monitor_goal_.target_link, monitor_goal_.joint_trajectory); 00650 head_controller_action_client_.sendGoal(goal); 00651 //Setting timer for actually sending trajectory 00652 start_trajectory_timer_ = root_handle_.createTimer(ros::Duration(monitor_goal_.time_offset), boost::bind(&HeadMonitor::trajectoryTimerCallback, this), true); 00653 } else { 00654 //calling this directly 00655 trajectoryTimerCallback(); 00656 } 00657 } 00658 00659 void stopHead() 00660 { 00661 head_controller_action_client_.cancelAllGoals(); 00662 } 00663 00664 void stopArm() 00665 { 00666 if(current_execution_status_.status == current_execution_status_.EXECUTING) { 00667 current_arm_controller_action_client_->cancelGoal(); 00668 } 00669 } 00670 00671 void stopEverything() { 00672 start_trajectory_timer_.stop(); 00673 paused_callback_timer_.stop(); 00674 stopHead(); 00675 stopArm(); 00676 } 00677 00678 void monitorPreemptCallback() 00679 { 00680 mutex_.lock(); 00681 if(current_execution_status_.status != current_execution_status_.MONITOR_BEFORE_EXECUTION && 00682 current_execution_status_.status != current_execution_status_.EXECUTING && 00683 current_execution_status_.status != current_execution_status_.PAUSED) { 00684 ROS_WARN_STREAM("Got preempt not in a relevant mode"); 00685 return; 00686 } 00687 mutex_.unlock(); 00688 //no reason not to cancel 00689 stopEverything(); 00690 00691 mutex_.lock(); 00692 current_execution_status_.status = current_execution_status_.IDLE; 00693 00694 ROS_INFO_STREAM(ros::this_node::getName() << ": Preempted"); 00695 head_monitor_action_server_.setPreempted(monitor_result_); 00696 mutex_.unlock(); 00697 } 00698 00699 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) { 00700 last_joint_state_ = *joint_state; 00701 if(current_execution_status_.status == current_execution_status_.IDLE 00702 || current_execution_status_.status == current_execution_status_.PREPLAN_SCAN) { 00703 return; 00704 } 00705 std::map<std::string, double> joint_state_map; 00706 std::map<std::string, double> joint_velocity_map; 00707 //message already been validated in kmsm 00708 for (unsigned int i = 0 ; i < joint_state->position.size(); ++i) 00709 { 00710 joint_state_map[joint_state->name[i]] = joint_state->position[i]; 00711 joint_velocity_map[joint_state->name[i]] = joint_state->velocity[i]; 00712 } 00713 trajectory_msgs::JointTrajectoryPoint point; 00714 point.positions.resize(logged_trajectory_.joint_names.size()); 00715 point.velocities.resize(logged_trajectory_.joint_names.size()); 00716 for(unsigned int i = 0; i < logged_trajectory_.joint_names.size(); i++) { 00717 point.positions[i] = joint_state_map[logged_trajectory_.joint_names[i]]; 00718 point.velocities[i] = joint_velocity_map[logged_trajectory_.joint_names[i]]; 00719 } 00720 point.time_from_start = ros::Time::now()-logged_trajectory_start_time_; 00721 logged_trajectory_.points.push_back(point); 00722 } 00723 00724 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state, 00725 const control_msgs::FollowJointTrajectoryResultConstPtr& result) 00726 { 00727 mutex_.lock(); 00728 if(current_execution_status_.status != current_execution_status_.EXECUTING) { 00729 //because we cancelled 00730 mutex_.unlock(); 00731 return; 00732 } 00733 00734 ROS_INFO_STREAM("Trajectory reported done with state " << state.toString()); 00735 current_execution_status_.status = current_execution_status_.IDLE; 00736 monitor_result_.actual_trajectory = logged_trajectory_; 00737 monitor_result_.error_code.val = monitor_result_.error_code.SUCCESS; 00738 head_monitor_action_server_.setSucceeded(monitor_result_); 00739 mutex_.unlock(); 00740 } 00741 00742 void pauseTimeoutCallback() { 00743 ROS_INFO_STREAM("Paused trajectory timed out"); 00744 stopEverything(); 00745 mutex_.lock(); 00746 current_execution_status_.status = current_execution_status_.IDLE; 00747 monitor_result_.actual_trajectory = logged_trajectory_; 00748 monitor_result_.error_code.val = monitor_result_.error_code.TIMED_OUT; 00749 head_monitor_action_server_.setAborted(monitor_result_); 00750 mutex_.unlock(); 00751 } 00752 00756 00757 00758 void trajectoryTimerCallback() { 00759 current_execution_status_.status = current_execution_status_.EXECUTING; 00760 00761 control_msgs::FollowJointTrajectoryGoal goal; 00762 00763 sensor_msgs::JointState js = last_joint_state_; 00764 planning_environment::removeCompletedTrajectory(collision_models_interface_->getParsedDescription(), 00765 monitor_goal_.joint_trajectory, 00766 js, 00767 goal.trajectory, 00768 false); 00769 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2); 00770 00771 ROS_INFO("Sending trajectory with %d points (excerpted from %d points) and timestamp: %f",(int)goal.trajectory.points.size(),(int) monitor_goal_.joint_trajectory.points.size(), goal.trajectory.header.stamp.toSec()); 00772 current_arm_controller_action_client_->sendGoal(goal,boost::bind(&HeadMonitor::controllerDoneCallback, this, _1, _2)); 00773 } 00774 00778 00779 trajectory_msgs::JointTrajectory generateHeadTrajectory(const std::string& link, const trajectory_msgs::JointTrajectory& joint_trajectory) { 00780 trajectory_msgs::JointTrajectory ret_traj; 00781 ret_traj.header = joint_trajectory.header; 00782 ret_traj.header.stamp = ros::Time::now(); 00783 ret_traj.joint_names.resize(2); 00784 ret_traj.joint_names[0] = "head_pan_joint"; 00785 ret_traj.joint_names[1] = "head_tilt_joint"; 00786 00787 //TODO - incorporate Adam's stuff 00788 00789 ret_traj.points.resize(2); 00790 00791 ret_traj.points[0].positions.resize(2); 00792 ret_traj.points[1].positions.resize(2); 00793 00794 ret_traj.points[0].positions[0] = start_head_pan_; 00795 ret_traj.points[0].positions[1] = start_head_tilt_; 00796 ret_traj.points[0].time_from_start = ros::Duration(.2); 00797 00798 ret_traj.points[1].positions[0] = goal_head_pan_; 00799 ret_traj.points[1].positions[1] = goal_head_tilt_; 00800 ret_traj.points[1].time_from_start = joint_trajectory.points.back().time_from_start; 00801 00802 return ret_traj; 00803 } 00804 00805 // Points the head at a point in a given frame 00806 void lookAt(std::string frame_id, double x, double y, double z, bool wait) 00807 { 00808 00809 pr2_controllers_msgs::PointHeadGoal goal; 00810 00811 //the target point, expressed in the requested frame 00812 geometry_msgs::PointStamped point; 00813 point.header.frame_id = frame_id; 00814 point.point.x = x; 00815 point.point.y = y; 00816 point.point.z = z; 00817 00818 goal.target = point; 00819 00820 //take at least 0.4 seconds to get there, lower value makes head jerky 00821 goal.min_duration = ros::Duration(.4); 00822 goal.max_velocity = 0.4; 00823 00824 if(wait) 00825 { 00826 if(point_head_action_client_.sendGoalAndWait(goal, ros::Duration(15.0), ros::Duration(0.5)) != actionlib::SimpleClientGoalState::SUCCEEDED) 00827 ROS_WARN("Point head timed out, continuing"); 00828 } 00829 else 00830 { 00831 point_head_action_client_.sendGoal(goal); 00832 } 00833 } 00834 }; 00835 00836 00837 int main(int argc, char** argv) 00838 { 00839 ros::init(argc, argv, "move_arm_head_monitor"); 00840 00841 ros::AsyncSpinner spinner(3); 00842 spinner.start(); 00843 00844 HeadMonitor head_monitor; 00845 00846 ros::waitForShutdown(); 00847 return 0; 00848 } 00849