pr2_head_action.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <cmath>
00036 
00037 #include <ros/ros.h>
00038 #include <message_filters/subscriber.h>
00039 #include <tf/message_filter.h>
00040 #include <tf/transform_listener.h>
00041 #include <actionlib/server/action_server.h>
00042 
00043 #include <geometry_msgs/PointStamped.h>
00044 #include <trajectory_msgs/JointTrajectory.h>
00045 #include <pr2_controllers_msgs/PointHeadAction.h>
00046 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00047 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00048 
00049 class ControlHead
00050 {
00051 private:
00052   typedef actionlib::ActionServer<pr2_controllers_msgs::PointHeadAction> PHAS;
00053   typedef PHAS::GoalHandle GoalHandle;
00054 public:
00055   ControlHead(const ros::NodeHandle &n)
00056     : node_(n),
00057       action_server_(node_, "point_head_action",
00058                      boost::bind(&ControlHead::goalCB, this, _1),
00059                      boost::bind(&ControlHead::cancelCB, this, _1)),
00060       has_active_goal_(false)
00061   {
00062     ros::NodeHandle pn("~");
00063     pn.param("pan_link", pan_link_, std::string("head_pan_link"));
00064     pn.param("tilt_link", tilt_link_, std::string("head_tilt_link"));
00065     pn.param("success_angle_threshold", success_angle_threshold_, 0.1);
00066 
00067     // \todo Need to actually look these joints up
00068     pan_joint_ = "head_pan_joint";
00069     tilt_joint_ = "head_tilt_joint";
00070 
00071     std::vector<std::string> frames;
00072     frames.push_back(pan_link_);
00073     frames.push_back(tilt_link_);
00074 
00075     // Connects to the controller
00076     pub_controller_command_ =
00077       node_.advertise<trajectory_msgs::JointTrajectory>("command", 2);
00078     sub_controller_state_ =
00079       node_.subscribe("state", 1, &ControlHead::controllerStateCB, this);
00080     cli_query_traj_ =
00081       node_.serviceClient<pr2_controllers_msgs::QueryTrajectoryState>("query_state");
00082 
00083     watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &ControlHead::watchdog, this);
00084   }
00085 
00086 
00087   void goalCB(GoalHandle gh)
00088   {
00089     // Before we do anything, we need to know that name of the pan_link's parent.
00090     if (pan_parent_.empty())
00091     {
00092       for (int i = 0; i < 10; ++i)
00093       {
00094         try {
00095           tf_.getParent(pan_link_, ros::Time(), pan_parent_);
00096           break;
00097         }
00098         catch (const tf::TransformException &ex) {}
00099         ros::Duration(0.5).sleep();
00100       }
00101 
00102       if (pan_parent_.empty())
00103       {
00104         ROS_ERROR("Could not get parent of %s in the TF tree", pan_link_.c_str());
00105         gh.setRejected();
00106         return;
00107       }
00108     }
00109 
00110 
00111     std::vector<double> q_goal(2);  // [pan, tilt]
00112 
00113     // Transforms the target point into the pan and tilt links.
00114     const geometry_msgs::PointStamped &target = gh.getGoal()->target;
00115     bool ret1 = false, ret2 = false;
00116     try {
00117       ros::Time now = ros::Time::now();
00118       std::string error_msg;
00119       ret1 = tf_.waitForTransform(pan_parent_, target.header.frame_id, target.header.stamp,
00120                                  ros::Duration(5.0), ros::Duration(0.01), &error_msg);
00121       ret2 = tf_.waitForTransform(pan_link_, target.header.frame_id, target.header.stamp,
00122                                    ros::Duration(5.0), ros::Duration(0.01), &error_msg);
00123 
00124       // Performs IK to determine the desired joint angles
00125 
00126       // Transforms the target into the pan and tilt frames
00127       tf::Stamped<tf::Point> target_point, target_in_tilt;
00128       tf::pointStampedMsgToTF(target, target_point);
00129       tf_.transformPoint(pan_parent_, target_point, target_in_pan_);
00130       tf_.transformPoint(pan_link_, target_point, target_in_tilt);
00131 
00132       // Computes the desired joint positions.
00133       q_goal[0] = atan2(target_in_pan_.y(), target_in_pan_.x());
00134       q_goal[1] = atan2(-target_in_tilt.z(),
00135                         sqrt(pow(target_in_tilt.x(),2) + pow(target_in_tilt.y(),2)));
00136     }
00137     catch(const tf::TransformException &ex)
00138     {
00139       ROS_ERROR("Transform failure (%d,%d): %s", ret1, ret2, ex.what());
00140       gh.setRejected();
00141       return;
00142     }
00143 
00144     // Queries where the movement should start.
00145     pr2_controllers_msgs::QueryTrajectoryState traj_state;
00146     traj_state.request.time = ros::Time::now() + ros::Duration(0.01);
00147     if (!cli_query_traj_.call(traj_state))
00148     {
00149       ROS_ERROR("Service call to query controller trajectory failed.");
00150       return;
00151     }
00152 
00153     if (has_active_goal_)
00154     {
00155       active_goal_.setCanceled();
00156       has_active_goal_ = false;
00157     }
00158 
00159     gh.setAccepted();
00160     active_goal_ = gh;
00161     has_active_goal_ = true;
00162 
00163     // Computes the duration of the movement.
00164 
00165     ros::Duration min_duration(0.01);
00166 
00167     if (gh.getGoal()->min_duration > min_duration)
00168         min_duration = gh.getGoal()->min_duration;
00169 
00170     // Determines if we need to increase the duration of the movement
00171     // in order to enforce a maximum velocity.
00172     if (gh.getGoal()->max_velocity > 0)
00173     {
00174       // Very approximate velocity limiting.
00175       double dist = sqrt(pow(q_goal[0] - traj_state.response.position[0], 2) +
00176                          pow(q_goal[1] - traj_state.response.position[1], 2));
00177       ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
00178       if (limit_from_velocity > min_duration)
00179         min_duration = limit_from_velocity;
00180     }
00181 
00182     // Computes the command to send to the trajectory controller.
00183     trajectory_msgs::JointTrajectory traj;
00184     traj.header.stamp = traj_state.request.time;
00185 
00186     traj.joint_names.push_back(pan_joint_);
00187     traj.joint_names.push_back(tilt_joint_);
00188 
00189     traj.points.resize(2);
00190     traj.points[0].positions = traj_state.response.position;
00191     traj.points[0].velocities = traj_state.response.velocity;
00192     traj.points[0].time_from_start = ros::Duration(0.0);
00193     traj.points[1].positions = q_goal;
00194     traj.points[1].velocities.push_back(0);
00195     traj.points[1].velocities.push_back(0);
00196     traj.points[1].time_from_start = ros::Duration(min_duration);
00197 
00198     pub_controller_command_.publish(traj);
00199   }
00200 
00201 
00202 private:
00203   std::string pan_link_;
00204   std::string tilt_link_;
00205   std::string pan_joint_;
00206   std::string tilt_joint_;
00207 
00208   ros::NodeHandle node_;
00209   tf::TransformListener tf_;
00210   ros::Publisher pub_controller_command_;
00211   ros::Subscriber sub_controller_state_;
00212   ros::Subscriber command_sub_;
00213   ros::ServiceClient cli_query_traj_;
00214   ros::Timer watchdog_timer_;
00215 
00216   PHAS action_server_;
00217   bool has_active_goal_;
00218   GoalHandle active_goal_;
00219   tf::Stamped<tf::Point> target_in_pan_;
00220   std::string pan_parent_;
00221   double success_angle_threshold_;
00222 
00223   void watchdog(const ros::TimerEvent &e)
00224   {
00225     ros::Time now = ros::Time::now();
00226 
00227     // Aborts the active goal if the controller does not appear to be active.
00228     if (has_active_goal_)
00229     {
00230       bool should_abort = false;
00231       if (!last_controller_state_)
00232       {
00233         should_abort = true;
00234         ROS_WARN("Aborting goal because we have never heard a controller state message.");
00235       }
00236       else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00237       {
00238         should_abort = true;
00239         ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00240                  (now - last_controller_state_->header.stamp).toSec());
00241       }
00242 
00243       if (should_abort)
00244       {
00245         // Stops the controller.
00246         trajectory_msgs::JointTrajectory empty;
00247         empty.joint_names.push_back(pan_joint_);
00248         empty.joint_names.push_back(tilt_joint_);
00249         pub_controller_command_.publish(empty);
00250 
00251         // Marks the current goal as aborted.
00252         active_goal_.setAborted();
00253         has_active_goal_ = false;
00254       }
00255     }
00256   }
00257 
00258   void cancelCB(GoalHandle gh)
00259   {
00260     if (active_goal_ == gh)
00261     {
00262       // Stops the controller.
00263       trajectory_msgs::JointTrajectory empty;
00264       empty.joint_names.push_back(pan_joint_);
00265       empty.joint_names.push_back(tilt_joint_);
00266       pub_controller_command_.publish(empty);
00267 
00268       // Marks the current goal as canceled.
00269       active_goal_.setCanceled();
00270       has_active_goal_ = false;
00271     }
00272   }
00273 
00274   pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
00275   void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
00276   {
00277     last_controller_state_ = msg;
00278     ros::Time now = ros::Time::now();
00279 
00280     if (!has_active_goal_)
00281       return;
00282 
00284     try
00285     {
00286       tf::Stamped<tf::Vector3> origin(tf::Vector3(0,0,0), msg->header.stamp, tilt_link_);
00287       tf::Stamped<tf::Vector3> forward(tf::Vector3(1,0,0), msg->header.stamp, tilt_link_);
00288       tf_.waitForTransform(pan_parent_, tilt_link_, msg->header.stamp, ros::Duration(1.0));
00289       tf_.transformPoint(pan_parent_, origin, origin);
00290       tf_.transformPoint(pan_parent_, forward, forward);
00291       pr2_controllers_msgs::PointHeadFeedback feedback;
00292       feedback.pointing_angle_error =
00293         (forward - origin).angle(target_in_pan_ - origin);
00294       active_goal_.publishFeedback(feedback);
00295 
00296       if (feedback.pointing_angle_error < success_angle_threshold_)
00297       {
00298         active_goal_.setSucceeded();
00299         has_active_goal_ = false;
00300       }
00301     }
00302     catch(const tf::TransformException &ex)
00303     {
00304       ROS_ERROR("Could not transform: %s", ex.what());
00305     }
00306   }
00307 
00308 };
00309 
00310 int main(int argc, char** argv)
00311 {
00312   ros::init(argc, argv, "point_head_action");
00313   ros::NodeHandle node;
00314   ControlHead ch(node);
00315   ros::spin();
00316   return 0;
00317 }


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