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


pr2_head_action
Author(s): Stuart Glaser
autogenerated on Fri Jan 3 2014 11:42:10