single_joint_position_action.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <cmath>
36 
37 #include <ros/ros.h>
39 
40 #include <trajectory_msgs/JointTrajectory.h>
41 #include <pr2_controllers_msgs/SingleJointPositionAction.h>
42 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
43 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
44 
46 {
47 private:
50 public:
52  : node_(n),
53  action_server_(node_, "position_joint_action",
54  boost::bind(&SingleJointPositionNode::goalCB, this, _1),
55  boost::bind(&SingleJointPositionNode::cancelCB, this, _1),
56  false),
57  has_active_goal_(false)
58  {
59  ros::NodeHandle pn("~");
60  if (!pn.getParam("joint", joint_))
61  {
62  ROS_FATAL("No joint given.");
63  exit(1);
64  }
65 
66  pn.param("goal_threshold", goal_threshold_, 0.1);
67  pn.param("max_acceleration", max_accel_, -1.0);
68 
69  // Connects to the controller
71  node_.advertise<trajectory_msgs::JointTrajectory>("command", 2);
75  node_.serviceClient<pr2_controllers_msgs::QueryTrajectoryState>("query_state");
76 
78  {
79  ROS_WARN("The controller does not appear to be ready (the query_state service is not available)");
80  }
81 
82  // Starts the action server
84 
86  }
87 
88 
89  //void pointHeadCB(const geometry_msgs::PointStampedConstPtr &msg)
90  void goalCB(GoalHandle gh)
91  {
92  // Queries where the movement should start.
93  pr2_controllers_msgs::QueryTrajectoryState traj_state;
94  traj_state.request.time = ros::Time::now() + ros::Duration(0.01);
95  if (!cli_query_traj_.call(traj_state))
96  {
97  ROS_ERROR("Service call to query controller trajectory failed. Service: (%s)",
98  cli_query_traj_.getService().c_str());
99  gh.setRejected();
100  return;
101  }
102 
103  if (has_active_goal_)
104  {
106  has_active_goal_ = false;
107  }
108 
109  gh.setAccepted();
110  active_goal_ = gh;
111  has_active_goal_ = true;
112 
113  // Computes the duration of the movement.
114 
115  ros::Duration min_duration(0.01);
116 
117  if (gh.getGoal()->min_duration > min_duration)
118  min_duration = gh.getGoal()->min_duration;
119 
120  // Determines if we need to increase the duration of the movement
121  // in order to enforce a maximum velocity.
122  if (gh.getGoal()->max_velocity > 0)
123  {
124  // Very approximate velocity limiting.
125  double dist = fabs(gh.getGoal()->position - traj_state.response.position[0]);
126  ros::Duration limit_from_velocity(dist / gh.getGoal()->max_velocity);
128  if (limit_from_velocity > min_duration)
129  min_duration = limit_from_velocity;
130  }
131 
132  // Computes the command to send to the trajectory controller.
133  trajectory_msgs::JointTrajectory traj;
134  traj.header.stamp = traj_state.request.time;
135 
136  traj.joint_names.push_back(joint_);
137 
138  traj.points.resize(2);
139  traj.points[0].positions = traj_state.response.position;
140  traj.points[0].velocities = traj_state.response.velocity;
141  traj.points[0].time_from_start = ros::Duration(0.0);
142  traj.points[1].positions.push_back(gh.getGoal()->position);
143  traj.points[1].velocities.push_back(0);
144  traj.points[1].time_from_start = ros::Duration(min_duration);
145 
147  }
148 
149 
150 private:
151  std::string joint_;
152  double max_accel_;
154 
161 
164  GoalHandle active_goal_;
165 
166  void watchdog(const ros::TimerEvent &e)
167  {
168  ros::Time now = ros::Time::now();
169 
170  // Aborts the active goal if the controller does not appear to be active.
171  if (has_active_goal_)
172  {
173  bool should_abort = false;
175  {
176  should_abort = true;
177  ROS_WARN("Aborting goal because we have never heard a controller state message.");
178  }
179  else if ((now - last_controller_state_->header.stamp) > ros::Duration(5.0))
180  {
181  should_abort = true;
182  ROS_WARN("Aborting goal because we haven't heard from the controller in %.3lf seconds",
183  (now - last_controller_state_->header.stamp).toSec());
184  }
185 
186  if (should_abort)
187  {
188  // Stops the controller.
189  trajectory_msgs::JointTrajectory empty;
190  empty.joint_names.push_back(joint_);
191  pub_controller_command_.publish(empty);
192 
193  // Marks the current goal as aborted.
194  active_goal_.setAborted();
195  has_active_goal_ = false;
196  }
197  }
198  }
199 
200  void cancelCB(GoalHandle gh)
201  {
202  if (active_goal_ == gh)
203  {
204  // Stops the controller.
205  trajectory_msgs::JointTrajectory empty;
206  empty.joint_names.push_back(joint_);
207  pub_controller_command_.publish(empty);
208 
209  // Marks the current goal as canceled.
210  active_goal_.setCanceled();
211  has_active_goal_ = false;
212  }
213  }
214 
215  pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_;
216  void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
217  {
218  last_controller_state_ = msg;
219  ros::Time now = ros::Time::now();
220 
221  if (!has_active_goal_)
222  return;
223 
224  pr2_controllers_msgs::SingleJointPositionFeedback feedback;
225  feedback.header.stamp = msg->header.stamp;
226  feedback.position = msg->actual.positions[0];
227  feedback.velocity = msg->actual.velocities[0];
228  feedback.error = msg->error.positions[0];
229  active_goal_.publishFeedback(feedback);
230 
231  if (fabs(msg->actual.positions[0] - active_goal_.getGoal()->position) < goal_threshold_)
232  {
233  active_goal_.setSucceeded();
234  has_active_goal_ = false;
235  }
236  }
237 
238 };
239 
240 int main(int argc, char** argv)
241 {
242  ros::init(argc, argv, "position_joint_action_node");
243  ros::NodeHandle node;
244  SingleJointPositionNode a(node);
245  ros::spin();
246  return 0;
247 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void publishFeedback(const Feedback &feedback)
int main(int argc, char **argv)
void controllerStateCB(const pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr &msg)
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
void setRejected(const Result &result=Result(), const std::string &text=std::string(""))
boost::shared_ptr< const Goal > getGoal() const
#define ROS_WARN(...)
actionlib::ActionServer< pr2_controllers_msgs::SingleJointPositionAction > SJPAS
pr2_controllers_msgs::JointTrajectoryControllerStateConstPtr last_controller_state_
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
void setAccepted(const std::string &text=std::string(""))
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
bool getParam(const std::string &key, std::string &s) const
static Time now()
std::string getService()
SingleJointPositionNode(const ros::NodeHandle &n)
void watchdog(const ros::TimerEvent &e)
#define ROS_ERROR(...)


single_joint_position_action
Author(s): Stuart Glaser
autogenerated on Wed Jun 5 2019 19:34:16