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