single_joint_position_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 <actionlib/server/action_server.h>
00039 
00040 #include <trajectory_msgs/JointTrajectory.h>
00041 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
00042 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00043 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00044 
00045 class SingleJointPositionNode
00046 {
00047 private:
00048   typedef actionlib::ActionServer<pr2_controllers_msgs::SingleJointPositionAction> SJPAS;
00049   typedef SJPAS::GoalHandle GoalHandle;
00050 public:
00051   SingleJointPositionNode(const ros::NodeHandle &n)
00052     : node_(n),
00053       action_server_(node_, "position_joint_action",
00054                      boost::bind(&SingleJointPositionNode::goalCB, this, _1),
00055                      boost::bind(&SingleJointPositionNode::cancelCB, this, _1),
00056                      false),
00057       has_active_goal_(false)
00058   {
00059     ros::NodeHandle pn("~");
00060     if (!pn.getParam("joint", joint_))
00061     {
00062       ROS_FATAL("No joint given.");
00063       exit(1);
00064     }
00065 
00066     pn.param("goal_threshold", goal_threshold_, 0.1);
00067     pn.param("max_acceleration", max_accel_, -1.0);
00068 
00069     // Connects to the controller
00070     pub_controller_command_ =
00071       node_.advertise<trajectory_msgs::JointTrajectory>("command", 2);
00072     sub_controller_state_ =
00073       node_.subscribe("state", 1, &SingleJointPositionNode::controllerStateCB, this);
00074     cli_query_traj_ =
00075       node_.serviceClient<pr2_controllers_msgs::QueryTrajectoryState>("query_state");
00076 
00077     if (!cli_query_traj_.waitForExistence(ros::Duration(10.0)))
00078     {
00079       ROS_WARN("The controller does not appear to be ready (the query_state service is not available)");
00080     }
00081 
00082     // Starts the action server
00083     action_server_.start();
00084 
00085     watchdog_timer_ = node_.createTimer(ros::Duration(1.0), &SingleJointPositionNode::watchdog, this);
00086   }
00087 
00088 
00089   //void pointHeadCB(const geometry_msgs::PointStampedConstPtr &msg)
00090   void goalCB(GoalHandle gh)
00091   {
00092     // Queries where the movement should start.
00093     pr2_controllers_msgs::QueryTrajectoryState traj_state;
00094     traj_state.request.time = ros::Time::now() + ros::Duration(0.01);
00095     if (!cli_query_traj_.call(traj_state))
00096     {
00097       ROS_ERROR("Service call to query controller trajectory failed.  Service: (%s)",
00098                 cli_query_traj_.getService().c_str());
00099       gh.setRejected();
00100       return;
00101     }
00102 
00103     if (has_active_goal_)
00104     {
00105       active_goal_.setCanceled();
00106       has_active_goal_ = false;
00107     }
00108 
00109     gh.setAccepted();
00110     active_goal_ = gh;
00111     has_active_goal_ = true;
00112 
00113     // Computes the duration of the movement.
00114 
00115     ros::Duration min_duration(0.01);
00116 
00117     if (gh.getGoal()->min_duration > min_duration)
00118         min_duration = gh.getGoal()->min_duration;
00119 
00120     // Determines if we need to increase the duration of the movement
00121     // in order to enforce a maximum velocity.
00122     if (gh.getGoal()->max_velocity > 0)
00123     {
00124       // Very approximate velocity limiting.
00125       double dist = fabs(gh.getGoal()->position - traj_state.response.position[0]);
00126       ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
00128       if (limit_from_velocity > min_duration)
00129         min_duration = limit_from_velocity;
00130     }
00131 
00132     // Computes the command to send to the trajectory controller.
00133     trajectory_msgs::JointTrajectory traj;
00134     traj.header.stamp = traj_state.request.time;
00135 
00136     traj.joint_names.push_back(joint_);
00137 
00138     traj.points.resize(2);
00139     traj.points[0].positions = traj_state.response.position;
00140     traj.points[0].velocities = traj_state.response.velocity;
00141     traj.points[0].time_from_start = ros::Duration(0.0);
00142     traj.points[1].positions.push_back(gh.getGoal()->position);
00143     traj.points[1].velocities.push_back(0);
00144     traj.points[1].time_from_start = ros::Duration(min_duration);
00145 
00146     pub_controller_command_.publish(traj);
00147   }
00148 
00149 
00150 private:
00151   std::string joint_;
00152   double max_accel_;
00153   double goal_threshold_;
00154 
00155   ros::NodeHandle node_;
00156   ros::Publisher pub_controller_command_;
00157   ros::Subscriber sub_controller_state_;
00158   ros::Subscriber command_sub_;
00159   ros::ServiceClient cli_query_traj_;
00160   ros::Timer watchdog_timer_;
00161 
00162   SJPAS action_server_;
00163   bool has_active_goal_;
00164   GoalHandle active_goal_;
00165 
00166   void watchdog(const ros::TimerEvent &e)
00167   {
00168     ros::Time now = ros::Time::now();
00169 
00170     // Aborts the active goal if the controller does not appear to be active.
00171     if (has_active_goal_)
00172     {
00173       bool should_abort = false;
00174       if (!last_controller_state_)
00175       {
00176         should_abort = true;
00177         ROS_WARN("Aborting goal because we have never heard a controller state message.");
00178       }
00179       else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
00180       {
00181         should_abort = true;
00182         ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
00183                  (now - last_controller_state_->header.stamp).toSec());
00184       }
00185 
00186       if (should_abort)
00187       {
00188         // Stops the controller.
00189         trajectory_msgs::JointTrajectory empty;
00190         empty.joint_names.push_back(joint_);
00191         pub_controller_command_.publish(empty);
00192 
00193         // Marks the current goal as aborted.
00194         active_goal_.setAborted();
00195         has_active_goal_ = false;
00196       }
00197     }
00198   }
00199 
00200   void cancelCB(GoalHandle gh)
00201   {
00202     if (active_goal_ == gh)
00203     {
00204       // Stops the controller.
00205       trajectory_msgs::JointTrajectory empty;
00206       empty.joint_names.push_back(joint_);
00207       pub_controller_command_.publish(empty);
00208 
00209       // Marks the current goal as canceled.
00210       active_goal_.setCanceled();
00211       has_active_goal_ = false;
00212     }
00213   }
00214 
00215   pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
00216   void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
00217   {
00218     last_controller_state_ = msg;
00219     ros::Time now = ros::Time::now();
00220 
00221     if (!has_active_goal_)
00222       return;
00223 
00224     pr2_controllers_msgs::SingleJointPositionFeedback feedback;
00225     feedback.header.stamp = msg->header.stamp;
00226     feedback.position = msg->actual.positions[0];
00227     feedback.velocity = msg->actual.velocities[0];
00228     feedback.error = msg->error.positions[0];
00229     active_goal_.publishFeedback(feedback);
00230 
00231     if (fabs(msg->actual.positions[0] - active_goal_.getGoal()->position) < goal_threshold_)
00232     {
00233       active_goal_.setSucceeded();
00234       has_active_goal_ = false;
00235     }
00236   }
00237 
00238 };
00239 
00240 int main(int argc, char** argv)
00241 {
00242   ros::init(argc, argv, "position_joint_action_node");
00243   ros::NodeHandle node;
00244   SingleJointPositionNode a(node);
00245   ros::spin();
00246   return 0;
00247 }


single_joint_position_action
Author(s): Stuart Glaser
autogenerated on Thu Apr 24 2014 15:44:28