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 #include <ros/ros.h>
00038 #include <actionlib/server/simple_action_server.h>
00039 #include <actionlib/client/simple_action_client.h>
00040
00041 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00042 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00043
00044 #include <angles/angles.h>
00045 #include <urdf/model.h>
00046 #include <urdf/joint.h>
00047
00048 #include <joint_trajectory_generator/trajectory_generation.h>
00049
00050 namespace joint_trajectory_generator {
00051 class JointTrajectoryGenerator
00052 {
00053 private:
00054 typedef actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> JTAS;
00055 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JTAC;
00056 public:
00057 JointTrajectoryGenerator(std::string name) :
00058 as_(ros::NodeHandle(), "joint_trajectory_generator",
00059 boost::bind(&JointTrajectoryGenerator::executeCb, this, _1),
00060 false),
00061 ac_("joint_trajectory_action"),
00062 got_state_(false)
00063 {
00064 ros::NodeHandle n;
00065 state_sub_ = n.subscribe("state", 1, &JointTrajectoryGenerator::jointStateCb, this);
00066
00067 ros::NodeHandle pn("~");
00068 pn.param("max_acc", max_acc_, 0.5);
00069 pn.param("max_vel", max_vel_, 5.0);
00070
00071
00072 ROS_DEBUG("Loading robot model");
00073 std::string xml_string;
00074 ros::NodeHandle nh_toplevel;
00075 if (!nh_toplevel.getParam(std::string("/robot_description"), xml_string))
00076 throw ros::Exception("Could not find paramter robot_description on parameter server");
00077
00078 if(!robot_model_.initString(xml_string))
00079 throw ros::Exception("Could not parse robot model");
00080
00081 ros::Rate r(10.0);
00082 while(!got_state_){
00083 ros::spinOnce();
00084 r.sleep();
00085 }
00086
00087 ac_.waitForServer();
00088 as_.start();
00089 ROS_INFO("%s: Initialized",name.c_str());
00090 }
00091
00092 void jointStateCb(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr& state){
00093 boost::mutex::scoped_lock lock(mutex_);
00094 for(unsigned int i = 0; i < state->joint_names.size(); ++i){
00095 current_state_[state->joint_names[i]] = state->actual.positions[i];
00096 }
00097 got_state_ = true;
00098 }
00099
00100 pr2_controllers_msgs::JointTrajectoryGoal createGoal(const pr2_controllers_msgs::JointTrajectoryGoal& goal){
00101 pr2_controllers_msgs::JointTrajectoryGoal new_goal;
00102 new_goal.trajectory.header = goal.trajectory.header;
00103 new_goal.trajectory.joint_names = goal.trajectory.joint_names;
00104
00105 size_t n_traj_points = goal.trajectory.points.size(),
00106 n_joint_names = goal.trajectory.joint_names.size();
00107
00108
00109 ROS_DEBUG_STREAM("Initial trajectory has "<<n_traj_points<<" points.");
00110 new_goal.trajectory.points.resize(n_traj_points + 1);
00111
00112
00113 for(size_t i=0; i<n_traj_points+1; i++) {
00114 new_goal.trajectory.points[i].positions.resize(n_joint_names);
00115 }
00116
00117 {
00118 boost::mutex::scoped_lock lock(mutex_);
00119
00120 for(unsigned int i = 0; i < n_joint_names; ++i){
00121
00122 if(current_state_.find(new_goal.trajectory.joint_names[i]) == current_state_.end()) {
00123 ROS_FATAL_STREAM("Joint names in goal and controller don't match. Something is very wrong. Goal joint name: "<<new_goal.trajectory.joint_names[i]);
00124 throw std::runtime_error("Joint names in goal and controller don't match. Something is very wrong.");
00125 }
00126 new_goal.trajectory.points[0].positions[i] = current_state_[new_goal.trajectory.joint_names[i]];
00127
00128
00129 boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(new_goal.trajectory.joint_names[i]);
00130 double offset = 0;
00131
00132 double goal_position = goal.trajectory.points[0].positions[i],
00133 current_position = new_goal.trajectory.points[0].positions[i];
00134
00135 if(joint->type == urdf::Joint::REVOLUTE) {
00136 offset = 0;
00137 } else if(joint->type == urdf::Joint::CONTINUOUS) {
00138 offset = current_position - goal_position - angles::shortest_angular_distance(goal_position, current_position);
00139 } else {
00140 ROS_WARN("Unknown joint type in joint trajectory. This joint might not be unwrapped properly. Supported joint types are urdf::Joint::REVOLUTE and urdf::Joint::CONTINUOUS");
00141 offset = 0;
00142 }
00143
00144
00145 for(unsigned int j=0; j < n_traj_points; j++) {
00146 new_goal.trajectory.points[j+1].time_from_start = goal.trajectory.points[j].time_from_start;
00147 new_goal.trajectory.points[j+1].positions[i] = goal.trajectory.points[j].positions[i] + offset;
00148 }
00149 }
00150 }
00151
00152
00153 trajectory::TrajectoryGenerator g(max_vel_, max_acc_, new_goal.trajectory.joint_names.size());
00154
00155
00156 g.generate(new_goal.trajectory, new_goal.trajectory);
00157
00158 return new_goal;
00159 }
00160
00161 void executeCb(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr& goal){
00162 ROS_DEBUG("Got a goal");
00163
00164 pr2_controllers_msgs::JointTrajectoryGoal full_goal;
00165 try {
00166 full_goal = createGoal(*goal);
00167 } catch (ros::Exception ex) {
00168 ROS_ERROR_STREAM(ex.what());
00169 as_.setAborted();
00170 return;
00171 }
00172
00173 ac_.sendGoal(full_goal, JTAC::SimpleDoneCallback(), JTAC::SimpleActiveCallback(), boost::bind(&JointTrajectoryGenerator::feedbackCb, this, _1));
00174
00175 while(ros::ok() && !ac_.waitForResult(ros::Duration(0.05))){
00176 if(as_.isPreemptRequested()){
00177 if(as_.isNewGoalAvailable()){
00178 ROS_DEBUG("Preempted by new goal");
00179 boost::shared_ptr<const pr2_controllers_msgs::JointTrajectoryGoal> new_goal = as_.acceptNewGoal();
00180 full_goal = createGoal(*new_goal);
00181 ac_.sendGoal(full_goal, JTAC::SimpleDoneCallback(),
00182 JTAC::SimpleActiveCallback(),
00183 boost::bind(&JointTrajectoryGenerator::feedbackCb, this, _1));
00184 }
00185 else{
00186 ROS_DEBUG("Preempted by cancel");
00187 ac_.cancelGoal();
00188 }
00189 }
00190 }
00191
00192 if(!ros::ok()){
00193 as_.setAborted();
00194 return;
00195 }
00196
00197 actionlib::SimpleClientGoalState state = ac_.getState();
00198 pr2_controllers_msgs::JointTrajectoryResultConstPtr result = ac_.getResult();
00199
00200 if(state == actionlib::SimpleClientGoalState::PREEMPTED){
00201 ROS_DEBUG("Preempted");
00202 as_.setPreempted(*result);
00203 }
00204 else if(state == actionlib::SimpleClientGoalState::SUCCEEDED){
00205 ROS_DEBUG("Succeeded ");
00206 as_.setSucceeded(*result);
00207 }
00208 else if(state == actionlib::SimpleClientGoalState::ABORTED){
00209 ROS_DEBUG("Aborted ");
00210 as_.setAborted(*result);
00211 }
00212 else
00213 as_.setAborted(*result, "Unknown result from joint_trajectory_action");
00214
00215 }
00216
00217 void feedbackCb(const pr2_controllers_msgs::JointTrajectoryFeedbackConstPtr& feedback){
00218 as_.publishFeedback(feedback);
00219 }
00220
00221 private:
00222 JTAS as_;
00223 JTAC ac_;
00224 boost::mutex mutex_;
00225 std::map<std::string, double> current_state_;
00226 ros::Subscriber state_sub_;
00227 bool got_state_;
00228 double max_acc_, max_vel_;
00229 urdf::Model robot_model_;
00230
00231 };
00232 };
00233
00234 int main(int argc, char** argv){
00235 ros::init(argc, argv, "joint_trajectory_generator_node");
00236 joint_trajectory_generator::JointTrajectoryGenerator jtg(ros::this_node::getName());
00237
00238 ros::spin();
00239
00240 return 0;
00241 }