$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #include <ros/ros.h> 00038 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h> 00039 #include <pr2_controllers_msgs/JointTrajectoryAction.h> 00040 #include <planning_environment/monitors/monitor_utils.h> 00041 #include <planning_environment/models/model_utils.h> 00042 #include <planning_environment/monitors/kinematic_model_state_monitor.h> 00043 #include <planning_environment/models/collision_models_interface.h> 00044 00045 #include <actionlib/server/simple_action_server.h> 00046 #include <actionlib/client/simple_action_client.h> 00047 #include <actionlib/client/simple_client_goal_state.h> 00048 00049 00050 #include <boost/thread/condition.hpp> 00051 #include <boost/scoped_ptr.hpp> 00052 #include <algorithm> 00053 #include <string> 00054 #include <limits> 00055 00056 namespace collision_free_arm_trajectory_controller 00057 { 00058 static const std::string TRAJECTORY_FILTER = "/trajectory_filter_server/filter_trajectory_with_constraints"; 00059 static const std::string TRAJECTORY_CONTROLLER = "/joint_trajectory_action"; 00060 static const double MIN_DELTA = 0.01; 00061 00062 enum ControllerState{ 00063 IDLE, 00064 START_CONTROL, 00065 MONITOR 00066 }; 00067 00068 00069 class CollisionFreeArmTrajectoryController 00070 { 00071 00072 public: 00073 CollisionFreeArmTrajectoryController(): private_handle_("~") 00074 { 00075 std::string robot_description_name = node_handle_.resolveName("robot_description", true); 00076 00077 collision_models_interface_ = new planning_environment::CollisionModelsInterface(robot_description_name); 00078 kmsm_ = new planning_environment::KinematicModelStateMonitor(collision_models_interface_, &tf_); 00079 00080 kmsm_->addOnStateUpdateCallback(boost::bind(&CollisionFreeArmTrajectoryController::jointStateCallback, this, _1)); 00081 00082 collision_models_interface_->addSetPlanningSceneCallback(boost::bind(&CollisionFreeArmTrajectoryController::setPlanningSceneCallback, this, _1)); 00083 00084 ros::service::waitForService("filter_trajectory"); 00085 00086 filter_trajectory_client_ = node_handle_.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>("filter_trajectory",true); 00087 00088 private_handle_.param<std::string>("group_name", group_name_, ""); 00089 00090 traj_action_client_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>(TRAJECTORY_CONTROLLER, true); 00091 while(ros::ok() && !traj_action_client_->waitForServer(ros::Duration(1.0))){ 00092 ROS_INFO("Waiting for the arm trajectory controller to come up"); 00093 } 00094 00095 action_server_.reset(new actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction>(node_handle_, "collision_free_arm_trajectory_action_" + group_name_, false)); 00096 00097 action_server_->registerGoalCallback(boost::bind(&CollisionFreeArmTrajectoryController::executeTrajectory, this)); 00098 action_server_->start(); 00099 state_ = IDLE; 00100 } 00101 00102 ~CollisionFreeArmTrajectoryController() 00103 { 00104 delete traj_action_client_; 00105 delete collision_models_interface_; 00106 delete kmsm_; 00107 } 00108 00109 void setPlanningSceneCallback(const arm_navigation_msgs::PlanningScene& scene) { 00110 collision_models_interface_->bodiesLock(); 00111 collision_models_interface_->disableCollisionsForNonUpdatedLinks(group_name_); 00112 collision_models_interface_->bodiesUnlock(); 00113 } 00114 00115 void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state) { 00116 00117 collision_models_interface_->bodiesLock(); 00118 if(!collision_models_interface_->getPlanningSceneState()) { 00119 if(state_ == MONITOR) { 00120 ROS_WARN("Should be monitoring, but no planning scene set"); 00121 traj_action_client_->cancelAllGoals(); 00122 action_server_->setAborted(); 00123 state_ = IDLE; 00124 } 00125 collision_models_interface_->bodiesUnlock(); 00126 return; 00127 } 00128 kmsm_->setStateValuesFromCurrentValues(*collision_models_interface_->getPlanningSceneState()); 00129 if(state_ != MONITOR || current_joint_trajectory_.points.size() == 0) { 00130 collision_models_interface_->bodiesUnlock(); 00131 return; 00132 } 00133 trajectory_msgs::JointTrajectory joint_trajectory_subset; 00134 planning_environment::removeCompletedTrajectory(collision_models_interface_->getParsedDescription(), 00135 current_joint_trajectory_, 00136 *joint_state, 00137 joint_trajectory_subset, 00138 false); 00139 current_joint_trajectory_ = joint_trajectory_subset; 00140 arm_navigation_msgs::Constraints emp; 00141 arm_navigation_msgs::ArmNavigationErrorCodes error_code; 00142 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> error_code_vec; 00143 if(current_joint_trajectory_.points.size() > 0) { 00144 if(!collision_models_interface_->isJointTrajectoryValid(*collision_models_interface_->getPlanningSceneState(), 00145 current_joint_trajectory_, 00146 emp, emp, 00147 error_code, 00148 error_code_vec, 00149 false)) { 00150 ROS_WARN_STREAM("Collision at point " << error_code_vec.size() << " of remaining trajectory points " << current_joint_trajectory_.points.size()); 00151 traj_action_client_->cancelAllGoals(); 00152 action_server_->setAborted(); 00153 state_ = IDLE; 00154 } 00155 } 00156 collision_models_interface_->bodiesUnlock(); 00157 } 00158 00159 void executeTrajectory() 00160 { 00161 if(state_ != IDLE) { 00162 ROS_INFO_STREAM("Preempted, so stopping"); 00163 traj_action_client_->cancelAllGoals(); 00164 } 00165 pr2_controllers_msgs::JointTrajectoryGoal goal(*(action_server_->acceptNewGoal())); 00166 00167 ROS_INFO("Got trajectory with %d points and %d joints",(int)goal.trajectory.points.size(),(int)goal.trajectory.joint_names.size()); 00168 00169 collision_models_interface_->bodiesLock(); 00170 if(!collision_models_interface_->isPlanningSceneSet()) { 00171 ROS_WARN_STREAM("Can't execute safe trajectory control without planning scene"); 00172 action_server_->setAborted(); 00173 collision_models_interface_->bodiesUnlock(); 00174 return; 00175 } 00176 00177 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request req; 00178 arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response res; 00179 00180 planning_environment::convertKinematicStateToRobotState(*collision_models_interface_->getPlanningSceneState(), 00181 ros::Time::now(), 00182 collision_models_interface_->getWorldFrameId(), 00183 req.start_state); 00184 std::map<std::string, double> current_values; 00185 collision_models_interface_->getPlanningSceneState()->getKinematicStateValues(current_values); 00186 req.trajectory = goal.trajectory; 00187 00188 trajectory_msgs::JointTrajectoryPoint jtp; 00189 for(unsigned int i = 0; i < req.trajectory.joint_names.size(); i++) { 00190 ROS_DEBUG_STREAM("Setting joint " << req.trajectory.joint_names[i] << " value " << current_values[req.trajectory.joint_names[i]]); 00191 jtp.positions.push_back(current_values[req.trajectory.joint_names[i]]); 00192 } 00193 req.trajectory.points.insert(req.trajectory.points.begin(), jtp); 00194 req.group_name = group_name_; 00195 if(filter_trajectory_client_.call(req,res)) 00196 { 00197 if(res.error_code.val == res.error_code.SUCCESS) 00198 { 00199 current_joint_trajectory_ = res.trajectory; 00200 pr2_controllers_msgs::JointTrajectoryGoal goal; 00201 goal.trajectory = current_joint_trajectory_; 00202 goal.trajectory.header.stamp = ros::Time::now()+ros::Duration(0.2); 00203 traj_action_client_->sendGoal(goal,boost::bind(&CollisionFreeArmTrajectoryController::controllerDoneCallback, this, _1, _2)); 00204 state_ = MONITOR; 00205 return; 00206 } 00207 else 00208 { 00209 ROS_INFO_STREAM("Filter rejects trajectory based on current state"); 00210 action_server_->setAborted(); 00211 collision_models_interface_->bodiesUnlock(); 00212 return; 00213 } 00214 } else { 00215 ROS_WARN_STREAM("Filter trajectory call failed entirely"); 00216 action_server_->setAborted(); 00217 collision_models_interface_->bodiesUnlock(); 00218 return; 00219 } 00220 } 00221 00222 00223 void controllerDoneCallback(const actionlib::SimpleClientGoalState& state, 00224 const pr2_controllers_msgs::JointTrajectoryResultConstPtr& result) 00225 { 00226 ROS_INFO_STREAM("Trajectory reported done with state " << state.toString()); 00227 state_ = IDLE; 00228 action_server_->setSucceeded(); 00229 } 00230 00231 private: 00232 ros::NodeHandle node_handle_, private_handle_; 00233 00234 std::string group_name_; 00235 00236 planning_environment::CollisionModelsInterface* collision_models_interface_; 00237 planning_environment::KinematicModelStateMonitor* kmsm_; 00238 00239 ros::ServiceClient filter_trajectory_client_; 00240 00241 tf::TransformListener tf_; 00242 00243 boost::shared_ptr<actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> > action_server_; 00244 actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* traj_action_client_; 00245 00246 ControllerState state_; 00247 trajectory_msgs::JointTrajectory current_joint_trajectory_; 00248 }; 00249 } 00250 00251 int main(int argc, char** argv) 00252 { 00253 ros::init(argc, argv, "collision_free_arm_trajectory_controller"); 00254 collision_free_arm_trajectory_controller::CollisionFreeArmTrajectoryController cf; 00255 ROS_INFO("Collision free arm trajectory controller started"); 00256 ros::spin(); 00257 return 0; 00258 } 00259