laser_scanner_traj_controller.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 
36 #include <algorithm>
37 #include <cmath>
38 #include "angles/angles.h"
40 
42 
43 using namespace std ;
44 using namespace controller ;
45 using namespace filters ;
46 
47 LaserScannerTrajController::LaserScannerTrajController() : traj_(1), d_error_filter_chain_("double")
48 {
49  tracking_offset_ = 0 ;
50  //track_link_enabled_ = false ;
51 }
52 
53 LaserScannerTrajController::~LaserScannerTrajController()
54 {
55 
56 }
57 
59 {
60  if (!robot)
61  return false ;
62  robot_ = robot ;
63 
64  // ***** Joint *****
65  string joint_name;
66  if (!n.getParam("joint", joint_name))
67  {
68  ROS_ERROR("LaserScannerTrajController: joint_name param not defined (namespace: %s)", n.getNamespace().c_str()) ;
69  return false;
70  }
71 
72  joint_state_ = robot_->getJointState(joint_name) ; // Need joint state to check 'calibrated' flag
73 
74  if (!joint_state_)
75  {
76  ROS_ERROR("LaserScannerTrajController: Could not find joint \"%s\" in robot model (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str()) ;
77  return false ;
78  }
79  if (!joint_state_->joint_->limits)
80  {
81  ROS_ERROR("LaserScannerTrajController: Joint \"%s\" has no limits specified (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str()) ;
82  return false ;
83  }
84 
85  // Fail if we're not calibrated. Is this better than checking it in the update loop? I'm not sure.
87  {
88  ROS_ERROR("LaserScannerTrajController: Could not start because joint [%s] isn't calibrated (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str());
89  return false;
90  }
91 
92  // ***** Gains *****
93  if (!pid_controller_.init(ros::NodeHandle(n, "gains")))
94  {
95  ROS_ERROR("LaserTiltController: Error initializing pid gains (namespace: %s)", n.getNamespace().c_str());
96  return false ;
97  }
98 
99  last_time_ = robot->getTime() ;
100  last_error_ = 0.0 ;
101 
102  // ***** Derivative Error Filter Element *****
103  if(!d_error_filter_chain_.configure("velocity_filter", n))
104  {
105  ROS_ERROR("LaserTiltController: Error initializing filter chain");
106  return false;
107  }
108 
109  // ***** Max Rate and Acceleration Elements *****
110  if (!n.getParam("max_velocity", max_rate_))
111  {
112  ROS_ERROR("max velocity param not defined");
113  return false;
114  }
115 
116  if (!n.getParam("max_acceleration", max_acc_))
117  {
118  ROS_ERROR("max acceleration param not defined");
119  return false;
120  }
121 
122  // Set to hold the current position
123 
124  pr2_msgs::PeriodicCmd cmd ;
125  cmd.profile = "linear" ;
126  cmd.period = 1.0 ;
127  cmd.amplitude = 0.0 ;
128  cmd.offset = joint_state_->position_ ;
129 
130  setPeriodicCmd(cmd) ;
131 
132  return true ;
133 }
134 
136 {
138  return;
139 
140  // ***** Compute the offset from tracking a link *****
142  /*if(track_link_lock_.try_lock())
143  {
144  if (track_link_enabled_ && target_link_ && mount_link_)
145  {
146  // Compute the position of track_point_ in the world frame
147  tf::Pose link_pose(target_link_->abs_orientation_, target_link_->abs_position_) ;
148  tf::Point link_point_world ;
149  link_point_world = link_pose*track_point_ ;
150 
151  // We're hugely approximating our inverse kinematics. This is probably good enough for now...
152  double dx = link_point_world.x() - mount_link_->abs_position_.x() ;
153  double dz = link_point_world.z() - mount_link_->abs_position_.z() ;
154  tracking_offset_ = atan2(-dz,dx) ;
155  }
156  else
157  {
158  tracking_offset_ = 0.0 ;
159  }
160  track_link_lock_.unlock() ;
161  }*/
162 
163  tracking_offset_ = 0.0 ;
164  // ***** Compute the current command from the trajectory profile *****
165  if (traj_lock_.try_lock())
166  {
167  if (traj_duration_ > 1e-6) // Short trajectories could make the mod_time calculation unstable
168  {
169  double profile_time = getCurProfileTime() ;
170 
171  trajectory::Trajectory::TPoint sampled_point ;
172  sampled_point.dimension_ = 1 ;
173  sampled_point.q_.resize(1) ;
174  sampled_point.qdot_.resize(1) ;
175  int result ;
176 
177  result = traj_.sample(sampled_point, profile_time) ;
178  if (result > 0)
179  traj_command_ = sampled_point.q_[0] ;
180  }
181  traj_lock_.unlock() ;
182  }
183 
184  // ***** Run the position control loop *****
185  double cmd = traj_command_ + tracking_offset_ ;
186 
187  ros::Time time = robot_->getTime();
188  double error(0.0) ;
190  joint_state_->joint_->limits->lower,
191  joint_state_->joint_->limits->upper,
192  error) ;
193  ros::Duration dt = time - last_time_ ;
194  double d_error = (error - last_error_)/dt.toSec();
195  double filtered_d_error;
196 
197  // Weird that we're filtering the d_error. Probably makes more sense to filter the velocity,
198  // and then compute (filtered_velocity - desired_velocity)
199  d_error_filter_chain_.update(d_error, filtered_d_error);
200 
201  // Update pid with d_error added
203  filtered_d_error, dt) ;
204  last_time_ = time ;
205  last_error_ = error ;
206 }
207 
209 {
210  ros::Time time = robot_->getTime();
211  double time_from_start = (time - traj_start_time_).toSec();
212  double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
213  return mod_time ;
214 }
215 
217 {
218  return traj_duration_ ;
219 }
220 
222 {
223  double cur_time = getCurProfileTime() ;
224  return traj_.findTrajectorySegment(cur_time) ;
225 }
226 
227 bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
228 {
229  while (!traj_lock_.try_lock())
230  usleep(100) ;
231 
232  vector<double> max_rates ;
233  max_rates.push_back(max_rate) ;
234  vector<double> max_accs ;
235  max_accs.push_back(max_acc) ;
236 
237 
238  traj_.autocalc_timing_ = true;
239 
240  traj_.setMaxRates(max_rates) ;
241  traj_.setMaxAcc(max_accs) ;
242  traj_.setInterpolationMethod(interp) ;
243 
244  traj_.setTrajectory(traj_points) ;
245 
247 
249 
250  traj_lock_.unlock() ;
251 
252  return true;
253 }
254 
255 bool LaserScannerTrajController::setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd)
256 {
257  if (cmd.profile == "linear" ||
258  cmd.profile == "blended_linear")
259  {
260  double high_pt = cmd.amplitude + cmd.offset ;
261  double low_pt = -cmd.amplitude + cmd.offset ;
262 
263 
264  double soft_limit_low = joint_state_->joint_->limits->lower;
265  double soft_limit_high = joint_state_->joint_->limits->upper;
266 
267  if (low_pt < soft_limit_low)
268  {
269  ROS_WARN("Lower setpoint (%.3f) is below the soft limit (%.3f). Truncating command", low_pt, soft_limit_low) ;
270  low_pt = soft_limit_low ;
271  }
272 
273  if (high_pt > soft_limit_high)
274  {
275  ROS_WARN("Upper setpoint (%.3f) is above the soft limit (%.3f). Truncating command", high_pt, soft_limit_high) ;
276  high_pt = soft_limit_high ;
277  }
278 
279  std::vector<trajectory::Trajectory::TPoint> tpoints ;
280 
281  trajectory::Trajectory::TPoint cur_point(1) ;
282 
283  cur_point.dimension_ = 1 ;
284 
285  cur_point.q_[0] = low_pt ;
286  cur_point.time_ = 0.0 ;
287  tpoints.push_back(cur_point) ;
288 
289  cur_point.q_[0] = high_pt ;
290  cur_point.time_ = cmd.period/2.0 ;
291  tpoints.push_back(cur_point) ;
292 
293  cur_point.q_[0] = low_pt ;
294  cur_point.time_ = cmd.period ;
295  tpoints.push_back(cur_point) ;
296 
297  if (!setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile)){
298  ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
299  return false;
300  }
301  else{
302  ROS_INFO("LaserScannerTrajController: Periodic Command set. Duration=%.4f sec", getProfileDuration()) ;
303  return true;
304  }
305  }
306  else
307  {
308  ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
309  return false;
310  }
311 }
312 
313 bool LaserScannerTrajController::setTrajCmd(const pr2_msgs::LaserTrajCmd& traj_cmd)
314 {
315  if (traj_cmd.profile == "linear" ||
316  traj_cmd.profile == "blended_linear")
317  {
318  const unsigned int N = traj_cmd.position.size() ;
319  if (traj_cmd.time_from_start.size() != N)
320  {
321  ROS_ERROR("# Times and # Pos must match! pos.size()=%u times.size()=%zu", N, traj_cmd.time_from_start.size()) ;
322  return false ;
323  }
324 
325  // Load up the trajectory data points, 1 point at a time
326  std::vector<trajectory::Trajectory::TPoint> tpoints ;
327  for (unsigned int i=0; i<N; i++)
328  {
329  trajectory::Trajectory::TPoint cur_point(1) ;
330  cur_point.dimension_ = 1 ;
331  cur_point.q_[0] = traj_cmd.position[i] ;
332  cur_point.time_ = traj_cmd.time_from_start[i].toSec() ;
333  tpoints.push_back(cur_point) ;
334  }
335 
336  double cur_max_rate = max_rate_ ;
337  double cur_max_acc = max_acc_ ;
338 
339  // Overwrite our limits, if they're specified in the msg. Is this maybe too dangerous?
340  if (traj_cmd.max_velocity > 0) // Only overwrite if a positive val
341  cur_max_rate = traj_cmd.max_velocity ;
342  if (traj_cmd.max_acceleration > 0)
343  cur_max_acc = traj_cmd.max_acceleration ;
344 
345  if (!setTrajectory(tpoints, cur_max_rate, cur_max_acc, traj_cmd.profile))
346  {
347  ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
348  return false;
349  }
350  else
351  {
352  ROS_INFO("LaserScannerTrajController: Trajectory Command set. Duration=%.4f sec", getProfileDuration()) ;
353  return true;
354  }
355  }
356  else
357  {
358  ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
359  return false;
360  }
361 }
362 
363 /*bool LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
364 {
365  while (!track_link_lock_.try_lock())
366  usleep(100) ;
367 
368  if (track_link_cmd.enable)
369  {
370  ROS_INFO("LaserScannerTrajController:: Tracking link %s", track_link_cmd.link_name.c_str()) ;
371  track_link_enabled_ = true ;
372  string mount_link_name = "laser_tilt_mount_link" ;
373  target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
374  mount_link_ = robot_->getLinkState(mount_link_name) ;
375  tf::pointMsgToTF(track_link_cmd.point, track_point_) ;
376 
377  if (target_link_ == NULL)
378  {
379  ROS_ERROR("LaserScannerTrajController:: Could not find target link:%s", track_link_cmd.link_name.c_str()) ;
380  track_link_enabled_ = false ;
381  }
382  if (mount_link_ == NULL)
383  {
384  ROS_ERROR("LaserScannerTrajController:: Could not find mount link:%s", mount_link_name.c_str()) ;
385  track_link_enabled_ = false ;
386  }
387  }
388  else
389  {
390  track_link_enabled_ = false ;
391  ROS_INFO("LaserScannerTrajController:: No longer tracking link") ;
392  }
393 
394  track_link_lock_.unlock() ;
395 
396  return track_link_enabled_;
397  }*/
398 
399 
401 {
402  prev_profile_segment_ = -1 ;
403  need_to_send_msg_ = false ; // Haven't completed a sweep yet, so don't need to send a msg
404  publisher_ = NULL ; // We don't know our topic yet, so we can't build it
405 }
406 
408 {
409  if (publisher_)
410  {
411  publisher_->stop();
412  delete publisher_; // Probably should wait on publish_->is_running() before exiting. Need to
413  // look into shutdown semantics for realtime_publisher
414  }
415 }
416 
418 {
419  c_.update() ;
420 
421  int cur_profile_segment = c_.getCurProfileSegment() ;
422 
423  if (cur_profile_segment != prev_profile_segment_)
424  {
425  // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
426  ros::Time cur_time(robot_->getTime()) ;
427  m_scanner_signal_.header.stamp = cur_time ;
428  m_scanner_signal_.signal = cur_profile_segment ;
430  }
431 
432  prev_profile_segment_ = cur_profile_segment ;
433 
434  // Use the realtime_publisher to try to send the message.
435  // If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
436  if (need_to_send_msg_)
437  {
438  if (publisher_->trylock())
439  {
441  publisher_->msg_.signal = m_scanner_signal_.signal ;
443  need_to_send_msg_ = false ;
444  }
445  }
446 }
447 
449  ros::NodeHandle &n)
450 {
451  robot_ = robot ; // Need robot in order to grab hardware time
452 
453  node_ = n;
454 
455  if (!c_.init(robot, n))
456  {
457  ROS_ERROR("Error Loading LaserScannerTrajController Params") ;
458  return false ;
459  }
460 
462  node_.subscribe("set_periodic_cmd", 1, &LaserScannerTrajControllerNode::setPeriodicCmd, this) ;
464  node_.subscribe("set_traj_cmd", 1, &LaserScannerTrajControllerNode::setTrajCmd, this) ;
465 
470 
471  if (publisher_ != NULL) // Make sure that we don't memory leak
472  {
473  ROS_ERROR("LaserScannerTrajController shouldn't ever execute this line... could be a bug elsewhere");
474  delete publisher_;
475  }
477 
478  prev_profile_segment_ = -1 ;
479 
480  ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
481 
482  return true ;
483 }
484 
485 
486 bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_msgs::SetPeriodicCmd::Request &req,
487  pr2_msgs::SetPeriodicCmd::Response &res)
488 {
489  ROS_INFO("LaserScannerTrajControllerNode: set periodic command");
490 
491  if (!c_.setPeriodicCmd(req.command))
492  return false;
493  else
494  {
495  res.start_time = ros::Time::now();
496  prev_profile_segment_ = -1 ;
497  return true;
498  }
499 }
500 
501 void LaserScannerTrajControllerNode::setPeriodicCmd(const pr2_msgs::PeriodicCmdConstPtr &cmd)
502 {
503  c_.setPeriodicCmd(*cmd) ;
504  prev_profile_segment_ = -1 ;
505 }
506 
507 bool LaserScannerTrajControllerNode::setTrajSrv(pr2_msgs::SetLaserTrajCmd::Request &req,
508  pr2_msgs::SetLaserTrajCmd::Response &res)
509 {
510  ROS_INFO("LaserScannerTrajControllerNode: set traj command");
511 
512  if (!c_.setTrajCmd(req.command))
513  return false;
514  else
515  {
516  res.start_time = ros::Time::now();
517  prev_profile_segment_ = -1 ;
518  return true;
519  }
520 }
521 
522 void LaserScannerTrajControllerNode::setTrajCmd(const pr2_msgs::LaserTrajCmdConstPtr &traj_cmd)
523 {
524  c_.setTrajCmd(*traj_cmd) ;
525  prev_profile_segment_ = -1 ;
526 }
527 
528 /*void LaserScannerTrajControllerNode::setTrackLinkCmd()
529 {
530  c_.setTrackLinkCmd(track_link_cmd_) ;
531  }*/
532 
PLUGINLIB_EXPORT_CLASS(my_controller_ns::MyControllerClass, pr2_controller_interface::Controller)
pr2_msgs::LaserScannerSignal m_scanner_signal_
Stores the message that we want to send at the end of each sweep, and halfway through each sweep...
filters::FilterChain< double > d_error_filter_chain_
Header header
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
double getTotalTime()
Get the total time for the trajectory.
Definition: trajectory.cpp:272
std::vector< double > q_
Definition: trajectory.h:72
int getCurProfileSegment()
Returns the current trajectory segment we&#39;re executing in our current profile.
void setPeriodicCmd(const pr2_msgs::PeriodicCmdConstPtr &cmd)
return true
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
boost::shared_ptr< const urdf::Joint > joint_
#define ROS_WARN(...)
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
double getProfileDuration()
Returns the length (in seconds) of our current profile.
pr2_mechanism_model::JointState * joint_state_
bool init(const ros::NodeHandle &n, const bool quiet=false)
bool setPeriodicCmd(const pr2_msgs::PeriodicCmd &cmd)
void setTrajCmd(const pr2_msgs::LaserTrajCmdConstPtr &traj_cmd)
double computeCommand(double error, ros::Duration dt)
#define ROS_INFO(...)
def error(args, kwargs)
bool need_to_send_msg_
Tracks whether we still need to send out the m_scanner_signal_ message.
const std::string & getNamespace() const
int prev_profile_segment_
The segment in the current profile when update() was last called.
JointState * getJointState(const std::string &name)
bool setTrajSrv(pr2_msgs::SetLaserTrajCmd::Request &req, pr2_msgs::SetLaserTrajCmd::Response &res)
def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, right_limit)
double getCurProfileTime()
Returns what time we&#39;re currently at in the profile being executed.
bool getParam(const std::string &key, std::string &s) const
bool setPeriodicSrv(pr2_msgs::SetPeriodicCmd::Request &req, pr2_msgs::SetPeriodicCmd::Response &res)
bool update(const T &data_in, T &data_out)
bool configure(std::string param_name, ros::NodeHandle node=ros::NodeHandle())
static Time now()
int sample(TPoint &tp, double time)
Sample the trajectory at a certain point in time.
Definition: trajectory.cpp:287
bool setTrajCmd(const pr2_msgs::LaserTrajCmd &traj_cmd)
int findTrajectorySegment(double time)
finds the trajectory segment corresponding to a particular time
Definition: trajectory.cpp:261
realtime_tools::RealtimePublisher< pr2_msgs::LaserScannerSignal > * publisher_
Publishes the m_scanner_signal msg from the update() realtime loop.
std::vector< double > qdot_
Definition: trajectory.h:74
#define ROS_ERROR(...)
int setMaxAcc(std::vector< double > max_acc)
Definition: trajectory.cpp:365
void setInterpolationMethod(std::string interp_method)
Set the interpolation method.
Definition: trajectory.cpp:810
bool setTrajectory(const std::vector< trajectory::Trajectory::TPoint > &traj_points, double max_rate, double max_acc, std::string interp)
int setMaxRates(std::vector< double > max_rate)
set the max rates (velocities)
Definition: trajectory.cpp:349
int setTrajectory(const std::vector< TPoint > &tp)
Set the trajectory using a vector of trajectory points.
Definition: trajectory.cpp:87


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Mon Jun 10 2019 14:26:33