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 #include <ros/ros.h>
00037
00038 #include <vector>
00039 #include <boost/scoped_ptr.hpp>
00040 #include <boost/thread/condition.hpp>
00041
00042 #include <actionlib/server/action_server.h>
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <geometry_msgs/PointStamped.h>
00045 #include <geometry_msgs/Twist.h>
00046
00047 #include <kdl/chainfksolver.hpp>
00048 #include <kdl/chain.hpp>
00049 #include <kdl/chainjnttojacsolver.hpp>
00050 #include <kdl/frames.hpp>
00051 #include "kdl/chainfksolverpos_recursive.hpp"
00052 #include "kdl_parser/kdl_parser.hpp"
00053
00054 #include "tf_conversions/tf_kdl.h"
00055 #include <message_filters/subscriber.h>
00056 #include <tf/message_filter.h>
00057 #include <tf/transform_datatypes.h>
00058 #include <tf/transform_listener.h>
00059
00060 #include <sensor_msgs/JointState.h>
00061 #include <std_msgs/Float32MultiArray.h>
00062 #include <std_msgs/Int32.h>
00063
00064 #include <urdf/model.h>
00065
00066 #include <trajectory_msgs/JointTrajectory.h>
00067 #include <pr2_controllers_msgs/PointHeadAction.h>
00068 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00069 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00070
00071
00072 void printVector3(const char * label, tf::Vector3 v)
00073 {
00074 printf("%s % 7.3f % 7.3f % 7.3f\n", label, v.x(), v.y(), v.z() );
00075
00076 }
00077
00078 class ControlHead
00079 {
00080 private:
00081 typedef actionlib::ActionServer<pr2_controllers_msgs::PointHeadAction> PHAS;
00082 typedef PHAS::GoalHandle GoalHandle;
00083
00084 std::string node_name_;
00085 std::string action_name_;
00086 std::string root_;
00087 std::string tip_;
00088 std::string pan_link_;
00089 std::string default_pointing_frame_;
00090 std::string pointing_frame_;
00091 tf::Vector3 pointing_axis_;
00092 std::vector< std::string> joint_names_;
00093
00094 ros::NodeHandle nh_, pnh_;
00095 ros::Publisher pub_controller_command_;
00096 ros::Subscriber sub_controller_state_;
00097 ros::Subscriber command_sub_;
00098 ros::ServiceClient cli_query_traj_;
00099 ros::Timer watchdog_timer_;
00100
00101 PHAS action_server_;
00102 bool has_active_goal_;
00103 GoalHandle active_goal_;
00104 tf::Stamped<tf::Point> target_in_pan_;
00105 std::string pan_parent_;
00106 double success_angle_threshold_;
00107
00108 geometry_msgs::PointStamped target_;
00109 KDL::Tree tree_;
00110 KDL::Chain chain_;
00111 tf::Point target_in_root_;
00112
00113 boost::scoped_ptr<KDL::ChainFkSolverPos> pose_solver_;
00114 boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_;
00115
00116 tf::TransformListener tfl_;
00117 urdf::Model urdf_model_;
00118
00119 public:
00120
00121 ControlHead(const ros::NodeHandle &node) :
00122 node_name_(ros::this_node::getName())
00123 , action_name_("point_head_action")
00124 , nh_(node)
00125 , pnh_("~")
00126 , action_server_(nh_, action_name_.c_str(),
00127 boost::bind(&ControlHead::goalCB, this, _1),
00128 boost::bind(&ControlHead::cancelCB, this, _1), false)
00129 , has_active_goal_(false)
00130 {
00131 pnh_.param("pan_link", pan_link_, std::string("head_pan_link"));
00132 pnh_.param("default_pointing_frame", default_pointing_frame_, std::string("head_tilt_link"));
00133 pnh_.param("success_angle_threshold", success_angle_threshold_, 0.1);
00134
00135 if(pan_link_[0] == '/') pan_link_.erase(0, 1);
00136 if(default_pointing_frame_[0] == '/') default_pointing_frame_.erase(0, 1);
00137
00138
00139 pub_controller_command_ =
00140 nh_.advertise<trajectory_msgs::JointTrajectory>("command", 2);
00141 sub_controller_state_ =
00142 nh_.subscribe("state", 1, &ControlHead::controllerStateCB, this);
00143 cli_query_traj_ =
00144 nh_.serviceClient<pr2_controllers_msgs::QueryTrajectoryState>("/pan_tilt_trajectory_controller/query_state");
00145
00146
00147 if(tree_.getNrOfJoints() == 0)
00148 {
00149 std::string robot_desc_string;
00150 nh_.param("/robot_description", robot_desc_string, std::string());
00151 ROS_DEBUG("Reading tree from robot_description...");
00152 if (!kdl_parser::treeFromString(robot_desc_string, tree_)){
00153 ROS_ERROR("Failed to construct kdl tree");
00154 exit(-1);
00155 }
00156 if (!urdf_model_.initString(robot_desc_string)){
00157 ROS_ERROR("Failed to parse urdf string for urdf::Model.");
00158 exit(-2);
00159 }
00160 }
00161
00162 ROS_DEBUG("Tree has %d joints and %d segments.", tree_.getNrOfJoints(), tree_.getNrOfSegments());
00163
00164 action_server_.start();
00165
00166 watchdog_timer_ = nh_.createTimer(ros::Duration(1.0), &ControlHead::watchdog, this);
00167 }
00168
00169 void goalCB(GoalHandle gh)
00170 {
00171
00172 if (root_.empty())
00173 {
00174 for (int i = 0; i < 10; ++i)
00175 {
00176 try {
00177 tfl_.getParent(pan_link_, ros::Time(), root_);
00178 break;
00179 }
00180 catch (const tf::TransformException &ex) {}
00181 ros::Duration(0.5).sleep();
00182 }
00183 if (root_.empty())
00184 {
00185 ROS_ERROR("Could not get parent of %s in the TF tree", pan_link_.c_str());
00186 gh.setRejected();
00187 return;
00188 }
00189 }
00190 if(root_[0] == '/') root_.erase(0, 1);
00191
00192 ROS_DEBUG("Got point head goal!");
00193
00194
00195 const geometry_msgs::PointStamped &target = gh.getGoal()->target;
00196 pointing_frame_ = gh.getGoal()->pointing_frame;
00197 if(pointing_frame_.length() == 0)
00198 {
00199 ROS_WARN("Pointing frame not specified, using %s [1, 0, 0] by default.", default_pointing_frame_.c_str());
00200 pointing_frame_ = default_pointing_frame_;
00201 pointing_axis_ = tf::Vector3(1.0, 0.0, 0.0);
00202 }
00203 else
00204 {
00205 if(pointing_frame_[0] == '/') pointing_frame_.erase(0, 1);
00206 bool ret1 = false;
00207 try
00208 {
00209 std::string error_msg;
00210 ret1 = tfl_.waitForTransform(pan_link_, pointing_frame_, target.header.stamp,
00211 ros::Duration(5.0), ros::Duration(0.01), &error_msg);
00212
00213 tf::vector3MsgToTF(gh.getGoal()->pointing_axis, pointing_axis_);
00214 if(pointing_axis_.length() < 0.1)
00215 {
00216 size_t found = pointing_frame_.find("optical_frame");
00217 if (found != std::string::npos)
00218 {
00219 ROS_WARN("Pointing axis appears to be zero-length. Using [0, 0, 1] as default for an optical frame.");
00220 pointing_axis_ = tf::Vector3(0, 0, 1);
00221 }
00222 else
00223 {
00224 ROS_WARN("Pointing axis appears to be zero-length. Using [1, 0, 0] as default for a non-optical frame.");
00225 pointing_axis_ = tf::Vector3(1, 0, 0);
00226 }
00227 }
00228 else
00229 {
00230 pointing_axis_.normalize();
00231 }
00232 }
00233 catch(const tf::TransformException &ex)
00234 {
00235 ROS_ERROR("Transform failure (%d): %s", ret1, ex.what());
00236 gh.setRejected();
00237 return;
00238 }
00239 }
00240
00241
00242 bool ret1 = false;
00243 try
00244 {
00245 std::string error_msg;
00246 ret1 = tfl_.waitForTransform(root_.c_str(), target.header.frame_id, target.header.stamp,
00247 ros::Duration(5.0), ros::Duration(0.01), &error_msg);
00248
00249 geometry_msgs::PointStamped target_in_root_msg;
00250 tfl_.transformPoint(root_.c_str(), target, target_in_root_msg );
00251 tf::pointMsgToTF(target_in_root_msg.point, target_in_root_);
00252 }
00253 catch(const tf::TransformException &ex)
00254 {
00255 ROS_ERROR("Transform failure (%d): %s", ret1, ex.what());
00256 gh.setRejected();
00257 return;
00258 }
00259
00260 if( tip_.compare(pointing_frame_) != 0 )
00261 {
00262 bool success = tree_.getChain(root_.c_str(), pointing_frame_.c_str(), chain_);
00263 if( !success )
00264 {
00265 ROS_ERROR("Couldn't create chain from %s to %s.", root_.c_str(), pointing_frame_.c_str());
00266 gh.setRejected();
00267 return;
00268 }
00269 tip_ = pointing_frame_;
00270
00271 pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
00272 jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
00273 joint_names_.resize(chain_.getNrOfJoints());
00274 }
00275
00276 unsigned int joints = chain_.getNrOfJoints();
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287 KDL::JntArray jnt_pos(joints), jnt_eff(joints);
00288 KDL::Jacobian jacobian(joints);
00289
00290 pr2_controllers_msgs::QueryTrajectoryState traj_state;
00291 traj_state.request.time = ros::Time::now() + ros::Duration(0.01);
00292 if (!cli_query_traj_.call(traj_state))
00293 {
00294 ROS_ERROR("Service call to query controller trajectory failed.");
00295 gh.setRejected();
00296 return;
00297 }
00298 if(traj_state.response.name.size() != joints)
00299 {
00300 ROS_ERROR("Number of joints mismatch: urdf chain vs. trajectory controller state.");
00301 gh.setRejected();
00302 return;
00303 }
00304 std::vector<urdf::JointLimits> limits_(joints);
00305
00306
00307 for(unsigned int i = 0; i < joints; i++)
00308 {
00309 joint_names_[i] = traj_state.response.name[i];
00310 limits_[i] = *(urdf_model_.joints_[joint_names_[i].c_str()]->limits);
00311 ROS_DEBUG("Joint %d %s: %f, limits: %f %f", i, traj_state.response.name[i].c_str(), traj_state.response.position[i], limits_[i].lower, limits_[i].upper);
00312
00313 jnt_pos(i) = 0;
00314 }
00315
00316 int count = 0;
00317 int limit_flips = 0;
00318 float correction_angle = 2*M_PI;
00319 float correction_delta = 2*M_PI;
00320 while( ros::ok() && fabs(correction_delta) > 0.001)
00321 {
00322
00323 KDL::Frame pose;
00324 pose_solver_->JntToCart(jnt_pos, pose);
00325 jac_solver_->JntToJac(jnt_pos, jacobian);
00326
00327 tf::Transform frame_in_root;
00328 tf::poseKDLToTF(pose, frame_in_root);
00329
00330 tf::Vector3 axis_in_frame = pointing_axis_.normalized();
00331 tf::Vector3 target_from_frame = (target_in_root_ - frame_in_root.getOrigin()).normalized();
00332 tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame;
00333 float prev_correction = correction_angle;
00334 correction_angle = current_in_frame.angle(axis_in_frame);
00335 correction_delta = correction_angle - prev_correction;
00336 ROS_DEBUG("At step %d, joint poses are %.4f and %.4f, angle error is %f", count, jnt_pos(0), jnt_pos(1), correction_angle);
00337 if(correction_angle < 0.5*success_angle_threshold_) break;
00338 tf::Vector3 correction_axis = frame_in_root.getBasis()*(axis_in_frame.cross(current_in_frame).normalized());
00339
00340 tf::Transform correction_tf(tf::Quaternion(correction_axis, 0.5*correction_angle), tf::Vector3(0,0,0));
00341 KDL::Frame correction_kdl;
00342 tf::transformTFToKDL(correction_tf, correction_kdl);
00343
00344
00345 KDL::Frame identity_kdl;
00346 KDL::Twist twist = diff(correction_kdl, identity_kdl);
00347 KDL::Wrench wrench_desi;
00348 for (unsigned int i=0; i<6; i++)
00349 wrench_desi(i) = -1.0*twist(i);
00350
00351
00352 for (unsigned int i = 0; i < joints; i++)
00353 {
00354 jnt_eff(i) = 0;
00355 for (unsigned int j=0; j<6; j++)
00356 jnt_eff(i) += (jacobian(j,i) * wrench_desi(j));
00357 jnt_pos(i) += jnt_eff(i);
00358 }
00359
00360
00361
00362
00363 for (unsigned int i = 0; i < joints; i++)
00364 {
00365 jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
00366 jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
00367 }
00368
00369
00370
00371
00372 count++;
00373
00374 if(limit_flips > 1){
00375 ROS_ERROR("Goal is out of joint limits, trying to point there anyway... \n");
00376 break;
00377 }
00378 }
00379 ROS_DEBUG("Iterative solver took %d steps", count);
00380
00381 std::vector<double> q_goal(joints);
00382
00383 for(unsigned int i = 0; i < joints; i++)
00384 {
00385 jnt_pos(i) = std::max(limits_[i].lower, jnt_pos(i));
00386 jnt_pos(i) = std::min(limits_[i].upper, jnt_pos(i));
00387 q_goal[i] = jnt_pos(i);
00388 ROS_DEBUG("Joint %d %s: %f", i, joint_names_[i].c_str(), jnt_pos(i));
00389 }
00390
00391 if (has_active_goal_)
00392 {
00393 active_goal_.setCanceled();
00394 has_active_goal_ = false;
00395 }
00396
00397 gh.setAccepted();
00398 active_goal_ = gh;
00399 has_active_goal_ = true;
00400
00401
00402
00403 ros::Duration min_duration(0.1);
00404
00405 if (gh.getGoal()->min_duration > min_duration)
00406 min_duration = gh.getGoal()->min_duration;
00407
00408
00409 if (gh.getGoal()->max_velocity > 0)
00410 {
00411
00412 double dist = sqrt(pow(q_goal[0] - traj_state.response.position[0], 2) +
00413 pow(q_goal[1] - traj_state.response.position[1], 2));
00414 ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
00415 if (limit_from_velocity > min_duration)
00416 min_duration = limit_from_velocity;
00417 }
00418
00419
00420
00421 trajectory_msgs::JointTrajectory traj;
00422 traj.header.stamp = traj_state.request.time;
00423
00424 traj.joint_names.push_back(traj_state.response.name[0]);
00425 traj.joint_names.push_back(traj_state.response.name[1]);
00426
00427 traj.points.resize(1);
00428
00429
00430
00431 traj.points[0].positions = q_goal;
00432 traj.points[0].velocities.push_back(0);
00433 traj.points[0].velocities.push_back(0);
00434 traj.points[0].time_from_start = ros::Duration(min_duration);
00435
00436 pub_controller_command_.publish(traj);
00437 }
00438
00439
00440 void watchdog(const ros::TimerEvent &e)
00441 {
00442 ros::Time now = ros::Time::now();
00443
00444
00445 if (has_active_goal_)
00446 {
00447 bool should_abort = false;
00448 if (!last_controller_state_)
00449 {
00450 should_abort = true;
00451 ROS_WARN("Aborting goal because we have never heard a controller state message.");
00452 }
00453 else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00454 {
00455 should_abort = true;
00456 ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00457 (now - last_controller_state_->header.stamp).toSec());
00458 }
00459
00460 if (should_abort)
00461 {
00462
00463 trajectory_msgs::JointTrajectory empty;
00464 empty.joint_names = joint_names_;
00465 pub_controller_command_.publish(empty);
00466
00467
00468 active_goal_.setAborted();
00469 has_active_goal_ = false;
00470 }
00471 }
00472 }
00473
00474
00475 void cancelCB(GoalHandle gh)
00476 {
00477 if (active_goal_ == gh)
00478 {
00479
00480 trajectory_msgs::JointTrajectory empty;
00481 empty.joint_names = joint_names_;
00482 pub_controller_command_.publish(empty);
00483
00484
00485 active_goal_.setCanceled();
00486 has_active_goal_ = false;
00487 }
00488 }
00489
00490 pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
00491 void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
00492 {
00493 last_controller_state_ = msg;
00494 ros::Time now = ros::Time::now();
00495
00496 if (!has_active_goal_)
00497 return;
00498
00500 try
00501 {
00502 KDL::JntArray jnt_pos(msg->joint_names.size());
00503 for(unsigned int i = 0; i < msg->joint_names.size(); i++)
00504 jnt_pos(i) = msg->actual.positions[i];
00505
00506 KDL::Frame pose;
00507 pose_solver_->JntToCart(jnt_pos, pose);
00508
00509 tf::Transform frame_in_root;
00510 tf::poseKDLToTF(pose, frame_in_root);
00511
00512 tf::Vector3 axis_in_frame = pointing_axis_.normalized();
00513 tf::Vector3 target_from_frame = target_in_root_ - frame_in_root.getOrigin();
00514
00515 target_from_frame.normalize();
00516 tf::Vector3 current_in_frame = frame_in_root.getBasis().inverse()*target_from_frame;
00517
00518 pr2_controllers_msgs::PointHeadFeedback feedback;
00519 feedback.pointing_angle_error = current_in_frame.angle(axis_in_frame);
00520 active_goal_.publishFeedback(feedback);
00521
00522 if (feedback.pointing_angle_error < success_angle_threshold_)
00523 {
00524 active_goal_.setSucceeded();
00525 has_active_goal_ = false;
00526 }
00527 }
00528 catch(const tf::TransformException &ex)
00529 {
00530 ROS_ERROR("Could not transform: %s", ex.what());
00531 }
00532 }
00533
00534
00535 };
00536
00537 int main(int argc, char** argv)
00538 {
00539 ros::init(argc, argv, "point_head_action");
00540 ros::NodeHandle node;
00541 ControlHead ch(node);
00542 ros::spin();
00543 return 0;
00544 }
00545