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
00037 #include <ros/ros.h>
00038 #include <actionlib/server/simple_action_server.h>
00039 #include <planning_environment_msgs/GetRobotState.h>
00040 #include <motion_planning_msgs/FilterJointTrajectory.h>
00041 #include <planning_environment_msgs/GetJointTrajectoryValidity.h>
00042 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00043 #include <actionlib/client/action_client.h>
00044
00045 #include <planning_environment/monitors/joint_state_monitor.h>
00046 #include <motion_planning_msgs/convert_messages.h>
00047
00048 #include <boost/thread/condition.hpp>
00049 #include <boost/scoped_ptr.hpp>
00050 #include <algorithm>
00051 #include <string>
00052 #include <limits>
00053
00054 namespace collision_free_arm_trajectory_controller
00055 {
00056 static const std::string TRAJECTORY_FILTER = "filter_trajectory";
00057 static const double MIN_DELTA = 0.01;
00058 typedef actionlib::ActionClient<pr2_controllers_msgs::JointTrajectoryAction> JointExecutorActionClient;
00059
00060 enum ControllerState{
00061 START_CONTROL,
00062 MONITOR
00063 };
00064
00065
00066 class CollisionFreeArmTrajectoryController
00067 {
00068
00069 public:
00070 CollisionFreeArmTrajectoryController(): private_handle_("~"), active_goal_(false)
00071 {
00072 traj_action_client_ = NULL;
00073 ros::service::waitForService("get_execution_safety");
00074 ros::service::waitForService("filter_trajectory");
00075 ros::service::waitForService("get_robot_state");
00076
00077 check_trajectory_validity_client_ = node_handle_.serviceClient<planning_environment_msgs::GetJointTrajectoryValidity>("get_execution_safety",true);
00078 filter_trajectory_client_ = node_handle_.serviceClient<motion_planning_msgs::FilterJointTrajectory>("filter_trajectory",true);
00079 get_state_client_ = node_handle_.serviceClient<planning_environment_msgs::GetRobotState>("get_robot_state",true);
00080
00081 std::string traj_action_name, group_name;
00082 private_handle_.param<std::string>("traj_action_name", traj_action_name, "action");
00083 private_handle_.param<std::string>("group_name", group_name, "");
00084 traj_action_client_ = new JointExecutorActionClient(traj_action_name);
00085 action_server_.reset(new actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction>(node_handle_, "collision_free_arm_trajectory_action_" + group_name, boost::bind(&CollisionFreeArmTrajectoryController::executeTrajectory, this, _1)));
00086 state_ = START_CONTROL;
00087 }
00088
00089 ~CollisionFreeArmTrajectoryController()
00090 {
00091 if(traj_action_client_)
00092 delete traj_action_client_;
00093 }
00094
00095
00096 bool execute(pr2_controllers_msgs::JointTrajectoryGoal &goal)
00097 {
00098 switch(state_)
00099 {
00100 case START_CONTROL:
00101 {
00102 ROS_DEBUG("Starting control");
00103 addCurrentState(goal);
00104 if(isTrajectoryValid(goal.trajectory))
00105 {
00106 goal.trajectory.header.stamp = ros::Time::now();
00107 sendGoalToController(goal);
00108 state_ = MONITOR;
00109 }
00110 else
00111 {
00112 traj_action_client_->cancelAllGoals();
00113 action_server_->setAborted();
00114 ROS_INFO("Aborting since trajectory is unsafe");
00115 }
00116 break;
00117 }
00118 case MONITOR:
00119 {
00120 if(!isTrajectoryValid(goal.trajectory))
00121 {
00122 traj_action_client_->cancelAllGoals();
00123 action_server_->setAborted();
00124 ROS_INFO("Aborting since trajectory is unsafe");
00125 }
00126 break;
00127 }
00128 default:
00129 {
00130 ROS_ERROR("Should not be here");
00131 break;
00132 }
00133 }
00134 if(!action_server_->isActive())
00135 {
00136 ROS_DEBUG("Controller no longer has an active goal");
00137 return true;
00138 }
00139 return false;
00140 }
00141
00142 void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal_input)
00143 {
00144 ROS_INFO("Got trajectory with %d points and %d joints",(int)goal_input->trajectory.points.size(),(int)goal_input->trajectory.joint_names.size());
00145
00146
00147 pr2_controllers_msgs::JointTrajectoryGoal goal = *goal_input;
00148 state_ = START_CONTROL;
00149 while(node_handle_.ok())
00150 {
00151 if(action_server_->isPreemptRequested())
00152 {
00153 if(action_server_->isNewGoalAvailable())
00154 {
00155 action_server_->setAborted();
00156 goal = *(action_server_->acceptNewGoal());
00157 state_ = START_CONTROL;
00158 }
00159 }
00160 bool done = execute(goal);
00161 if(done)
00162 return;
00163 ros::Duration(0.01).sleep();
00164 }
00165 ROS_INFO("Node was aborted");
00166 action_server_->setAborted();
00167 return;
00168 }
00169
00170
00171 bool isTrajectoryValid(const trajectory_msgs::JointTrajectory &traj)
00172 {
00173 planning_environment_msgs::GetJointTrajectoryValidity::Request req;
00174 planning_environment_msgs::GetJointTrajectoryValidity::Response res;
00175
00176 ROS_DEBUG("Received trajectory has %d points with %d joints",(int) traj.points.size(),(int)traj.joint_names.size());
00177 trajectory_msgs::JointTrajectory traj_discretized;
00178 discretizeTrajectory(traj,traj_discretized);
00179 req.trajectory = traj_discretized;
00180 ROS_DEBUG("Got robot state");
00181
00182 getRobotState(req.robot_state);
00183 req.check_path_constraints = true;
00184 req.check_collisions = true;
00185
00186 if(check_trajectory_validity_client_.call(req,res))
00187 {
00188 ROS_DEBUG("Service call to check plan validity succeeded");
00189 if(res.error_code.val == res.error_code.SUCCESS)
00190 return true;
00191 else
00192 {
00193 ROS_ERROR("Trajectory invalid. Error code: %d",res.error_code.val);
00194 return false;
00195 }
00196 }
00197 else
00198 {
00199 ROS_ERROR("Service call to check trajectory validity failed on %s",check_trajectory_validity_client_.getService().c_str());
00200 return false;
00201 }
00202 }
00203
00204 void sendGoalToController(const pr2_controllers_msgs::JointTrajectoryGoal &goal)
00205 {
00206 goal_handle_ = traj_action_client_->sendGoal(goal,boost::bind(&CollisionFreeArmTrajectoryController::transitionCallback, this, _1));
00207 }
00208
00209 void transitionCallback(JointExecutorActionClient::GoalHandle gh)
00210 {
00211 actionlib::CommState comm_state = gh.getCommState();
00212 if(comm_state.state_ == actionlib::CommState::DONE)
00213 {
00214 switch(gh.getTerminalState().state_)
00215 {
00216 case actionlib::TerminalState::RECALLED:
00217 case actionlib::TerminalState::REJECTED:
00218 case actionlib::TerminalState::PREEMPTED:
00219 case actionlib::TerminalState::ABORTED:
00220 case actionlib::TerminalState::LOST:
00221 ROS_DEBUG("Trajectory action did not succeed");
00222 action_server_->setAborted();
00223 return;
00224 case actionlib::TerminalState::SUCCEEDED:
00225 ROS_DEBUG("Reached goal");
00226 action_server_->setSucceeded();
00227 return;
00228 default:
00229 ROS_DEBUG("Unknown terminal state [%u]",gh.getTerminalState().state_);
00230 }
00231 }
00232 else
00233 ROS_DEBUG("Unknown comm state");
00234
00235 }
00236
00237 void discretizeTrajectory(const trajectory_msgs::JointTrajectory &trajectory, trajectory_msgs::JointTrajectory &trajectory_out)
00238 {
00239 trajectory_out.joint_names = trajectory.joint_names;
00240 for(unsigned int i=1; i < trajectory.points.size(); i++)
00241 {
00242 double diff = 0.0;
00243 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00244 {
00245 double start = trajectory.points[i-1].positions[j];
00246 double end = trajectory.points[i].positions[j];
00247 if(fabs(end-start) > diff)
00248 diff = fabs(end-start);
00249 }
00250 int num_intervals =(int) (diff/MIN_DELTA+1.0);
00251
00252 for(unsigned int k=0; k < (unsigned int) num_intervals; k++)
00253 {
00254 trajectory_msgs::JointTrajectoryPoint point;
00255 for(unsigned int j=0; j < trajectory.points[i].positions.size(); j++)
00256 {
00257 double start = trajectory.points[i-1].positions[j];
00258 double end = trajectory.points[i].positions[j];
00259 point.positions.push_back(start + (end-start)*k/num_intervals);
00260 }
00261 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);
00262 trajectory_out.points.push_back(point);
00263 }
00264 }
00265 trajectory_out.points.push_back(trajectory.points.back());
00266 }
00267
00268 bool filterTrajectory(trajectory_msgs::JointTrajectory &trajectory)
00269 {
00270 motion_planning_msgs::FilterJointTrajectory::Request req;
00271 motion_planning_msgs::FilterJointTrajectory::Response res;
00272 req.trajectory = trajectory;
00273 if(filter_trajectory_client_.call(req,res))
00274 {
00275 if(res.error_code.val == res.error_code.SUCCESS)
00276 {
00277 trajectory = res.trajectory;
00278 return true;
00279 }
00280 else
00281 {
00282 ROS_ERROR("Trajectory filtering failed");
00283 return false;
00284 }
00285 }
00286 else
00287 {
00288 ROS_ERROR("Service call to filter trajectory failed.");
00289 return false;
00290 }
00291
00292 }
00293
00294 bool addCurrentState(pr2_controllers_msgs::JointTrajectoryGoal &goal)
00295 {
00296
00297 double d = 0.0;
00298 sensor_msgs::JointState current = state_monitor_.getJointState(goal.trajectory.joint_names);
00299 for (unsigned int i = 0 ; i < current.position.size() ; ++i)
00300 {
00301 double dif = current.position[i] - goal.trajectory.points[0].positions[i];
00302 d = std::max<double>(d,fabs(dif));
00303 }
00304
00305 int include_first = (d > 0.1) ? 1 : 0;
00306 if (include_first)
00307 {
00308 trajectory_msgs::JointTrajectory start_segment;
00309 start_segment.points.resize(2);
00310 start_segment.points[0].positions = motion_planning_msgs::jointStateToJointTrajectoryPoint(current).positions;
00311 start_segment.points[0].time_from_start = ros::Duration(0.0);
00312 start_segment.points[1] = goal.trajectory.points[0];
00313 start_segment.joint_names = goal.trajectory.joint_names;
00314 if(!filterTrajectory(start_segment))
00315 ROS_WARN("Could not filter trajectory");
00316
00317 trajectory_msgs::JointTrajectory traj;
00318 traj.points = start_segment.points;
00319 traj.joint_names = start_segment.joint_names;
00320 for (unsigned int i = 1; i < goal.trajectory.points.size() ; ++i)
00321 {
00322 traj.points.push_back(goal.trajectory.points[i]);
00323 traj.points.back().time_from_start = goal.trajectory.points[i].time_from_start + start_segment.points.back().time_from_start;
00324 }
00325 goal.trajectory = traj;
00326 }
00327 return true;
00328 }
00329
00330 bool getRobotState(motion_planning_msgs::RobotState &robot_state)
00331 {
00332 planning_environment_msgs::GetRobotState::Request req;
00333 planning_environment_msgs::GetRobotState::Response res;
00334 if(get_state_client_.call(req,res))
00335 {
00336 robot_state = res.robot_state;
00337 return true;
00338 }
00339 else
00340 {
00341 ROS_ERROR("Service call to get robot state failed on %s",get_state_client_.getService().c_str());
00342 return false;
00343 }
00344 }
00345
00346 private:
00347 ros::NodeHandle node_handle_, private_handle_;
00348 JointExecutorActionClient *traj_action_client_;
00349 JointExecutorActionClient::GoalHandle goal_handle_;
00350 boost::shared_ptr<actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> > action_server_;
00351 ros::ServiceClient get_state_client_, check_trajectory_validity_client_, filter_trajectory_client_;
00352 planning_environment::JointStateMonitor state_monitor_;
00353 bool active_goal_;
00354 ControllerState state_;
00355 };
00356 }
00357
00358 int main(int argc, char** argv)
00359 {
00360 ros::init(argc, argv, "collision_free_arm_trajectory_controller");
00361 collision_free_arm_trajectory_controller::CollisionFreeArmTrajectoryController cf;
00362 ROS_INFO("Collision free arm trajectory controller started");
00363 ros::spin();
00364 return 0;
00365 }
00366