laser_scanner_traj_controller.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 "pr2_mechanism_controllers/laser_scanner_traj_controller.h"
00036 #include <algorithm>
00037 #include <cmath>
00038 #include "angles/angles.h"
00039 #include "pluginlib/class_list_macros.h"
00040 
00041 PLUGINLIB_EXPORT_CLASS( controller::LaserScannerTrajControllerNode, pr2_controller_interface::Controller)
00042 
00043 using namespace std ;
00044 using namespace controller ;
00045 using namespace filters ;
00046 
00047 LaserScannerTrajController::LaserScannerTrajController() : traj_(1), d_error_filter_chain_("double")
00048 {
00049   tracking_offset_ = 0 ;
00050   //track_link_enabled_ = false ;
00051 }
00052 
00053 LaserScannerTrajController::~LaserScannerTrajController()
00054 {
00055 
00056 }
00057 
00058 bool LaserScannerTrajController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle& n)
00059 {
00060   if (!robot)
00061     return false ;
00062   robot_ = robot ;
00063 
00064   // ***** Joint *****
00065   string joint_name;
00066   if (!n.getParam("joint", joint_name))
00067   {
00068     ROS_ERROR("LaserScannerTrajController: joint_name param not defined (namespace: %s)", n.getNamespace().c_str()) ;
00069     return false;
00070   }
00071 
00072   joint_state_ = robot_->getJointState(joint_name) ;  // Need joint state to check 'calibrated' flag
00073 
00074   if (!joint_state_)
00075   {
00076     ROS_ERROR("LaserScannerTrajController: Could not find joint \"%s\" in robot model (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str()) ;
00077     return false ;
00078   }
00079   if (!joint_state_->joint_->limits)
00080   {
00081     ROS_ERROR("LaserScannerTrajController: Joint \"%s\" has no limits specified (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str()) ;
00082     return false ;
00083   }
00084 
00085   // Fail if we're not calibrated. Is this better than checking it in the update loop?  I'm not sure.
00086   if (!joint_state_->calibrated_)
00087   {
00088     ROS_ERROR("LaserScannerTrajController: Could not start because joint [%s] isn't calibrated (namespace: %s)", joint_name.c_str(), n.getNamespace().c_str());
00089     return false;
00090   }
00091 
00092   // ***** Gains *****
00093   if (!pid_controller_.init(ros::NodeHandle(n, "gains")))
00094   {
00095     ROS_ERROR("LaserTiltController: Error initializing pid gains (namespace: %s)", n.getNamespace().c_str());
00096     return false ;
00097   }
00098 
00099   last_time_ = robot->getTime() ;
00100   last_error_ = 0.0 ;
00101 
00102   // ***** Derivative Error Filter Element *****
00103   if(!d_error_filter_chain_.configure("velocity_filter", n))
00104   {
00105     ROS_ERROR("LaserTiltController: Error initializing filter chain");
00106     return false;
00107   }
00108 
00109   // ***** Max Rate and Acceleration Elements *****
00110   if (!n.getParam("max_velocity", max_rate_))
00111   {
00112     ROS_ERROR("max velocity param not defined");
00113     return false;
00114   }
00115 
00116   if (!n.getParam("max_acceleration", max_acc_))
00117   {
00118     ROS_ERROR("max acceleration param not defined");
00119     return false;
00120   }
00121 
00122   // Set to hold the current position
00123 
00124   pr2_msgs::PeriodicCmd cmd ;
00125   cmd.profile = "linear" ;
00126   cmd.period = 1.0 ;
00127   cmd.amplitude = 0.0 ;
00128   cmd.offset = joint_state_->position_ ;
00129 
00130   setPeriodicCmd(cmd) ;
00131 
00132   return true ;
00133 }
00134 
00135 void LaserScannerTrajController::update()
00136 {
00137   if (!joint_state_->calibrated_)
00138     return;
00139 
00140   // ***** Compute the offset from tracking a link *****
00142   /*if(track_link_lock_.try_lock())
00143   {
00144     if (track_link_enabled_  && target_link_ && mount_link_)
00145     {
00146       // Compute the position of track_point_ in the world frame
00147       tf::Pose link_pose(target_link_->abs_orientation_, target_link_->abs_position_) ;
00148       tf::Point link_point_world ;
00149       link_point_world = link_pose*track_point_ ;
00150 
00151       // We're hugely approximating our inverse kinematics. This is probably good enough for now...
00152       double dx = link_point_world.x() - mount_link_->abs_position_.x() ;
00153       double dz = link_point_world.z() - mount_link_->abs_position_.z() ;
00154       tracking_offset_ = atan2(-dz,dx) ;
00155     }
00156     else
00157     {
00158       tracking_offset_ = 0.0 ;
00159     }
00160     track_link_lock_.unlock() ;
00161     }*/
00162 
00163   tracking_offset_ = 0.0 ;
00164   // ***** Compute the current command from the trajectory profile *****
00165   if (traj_lock_.try_lock())
00166   {
00167     if (traj_duration_ > 1e-6)                                   // Short trajectories could make the mod_time calculation unstable
00168     {
00169       double profile_time = getCurProfileTime() ;
00170 
00171       trajectory::Trajectory::TPoint sampled_point ;
00172       sampled_point.dimension_ = 1 ;
00173       sampled_point.q_.resize(1) ;
00174       sampled_point.qdot_.resize(1) ;
00175       int result ;
00176 
00177       result = traj_.sample(sampled_point, profile_time) ;
00178       if (result > 0)
00179         traj_command_ = sampled_point.q_[0] ;
00180     }
00181     traj_lock_.unlock() ;
00182   }
00183 
00184   // ***** Run the position control loop *****
00185   double cmd = traj_command_ + tracking_offset_ ;
00186 
00187   ros::Time time = robot_->getTime();
00188   double error(0.0) ;
00189   angles::shortest_angular_distance_with_limits(joint_state_->position_, cmd,
00190                                                 joint_state_->joint_->limits->lower,
00191                                                 joint_state_->joint_->limits->upper,
00192                                                 error) ;
00193   ros::Duration dt = time - last_time_ ;
00194   double d_error = (error - last_error_)/dt.toSec();
00195   double filtered_d_error;
00196 
00197   // Weird that we're filtering the d_error. Probably makes more sense to filter the velocity,
00198   //   and then compute (filtered_velocity - desired_velocity)
00199   d_error_filter_chain_.update(d_error, filtered_d_error);
00200 
00201   // Update pid with d_error added
00202   joint_state_->commanded_effort_ = pid_controller_.computeCommand(error, 
00203         filtered_d_error, dt) ;
00204   last_time_ = time ;
00205   last_error_ = error ;
00206 }
00207 
00208 double LaserScannerTrajController::getCurProfileTime()
00209 {
00210   ros::Time time = robot_->getTime();
00211   double time_from_start = (time - traj_start_time_).toSec();
00212   double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
00213   return mod_time ;
00214 }
00215 
00216 double LaserScannerTrajController::getProfileDuration()
00217 {
00218   return traj_duration_ ;
00219 }
00220 
00221 int LaserScannerTrajController::getCurProfileSegment()
00222 {
00223   double cur_time = getCurProfileTime() ;
00224   return traj_.findTrajectorySegment(cur_time) ;
00225 }
00226 
00227 bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
00228 {
00229   while (!traj_lock_.try_lock())
00230     usleep(100) ;
00231 
00232   vector<double> max_rates ;
00233   max_rates.push_back(max_rate) ;
00234   vector<double> max_accs ;
00235   max_accs.push_back(max_acc) ;
00236 
00237 
00238   traj_.autocalc_timing_ = true;
00239 
00240   traj_.setMaxRates(max_rates) ;
00241   traj_.setMaxAcc(max_accs) ;
00242   traj_.setInterpolationMethod(interp) ;
00243 
00244   traj_.setTrajectory(traj_points) ;
00245 
00246   traj_start_time_ = robot_->getTime() ;
00247 
00248   traj_duration_ = traj_.getTotalTime() ;
00249 
00250   traj_lock_.unlock() ;
00251 
00252   return true;
00253 }
00254 
00255 bool LaserScannerTrajController::setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd)
00256 {
00257   if (cmd.profile == "linear" ||
00258       cmd.profile == "blended_linear")
00259   {
00260     double high_pt = cmd.amplitude + cmd.offset ;
00261     double low_pt = -cmd.amplitude + cmd.offset ;
00262 
00263 
00264     double soft_limit_low  = joint_state_->joint_->limits->lower;
00265     double soft_limit_high = joint_state_->joint_->limits->upper;
00266 
00267     if (low_pt < soft_limit_low)
00268     {
00269       ROS_WARN("Lower setpoint (%.3f) is below the soft limit (%.3f). Truncating command", low_pt, soft_limit_low) ;
00270       low_pt = soft_limit_low ;
00271     }
00272 
00273     if (high_pt > soft_limit_high)
00274     {
00275       ROS_WARN("Upper setpoint (%.3f) is above the soft limit (%.3f). Truncating command", high_pt, soft_limit_high) ;
00276       high_pt = soft_limit_high ;
00277     }
00278 
00279     std::vector<trajectory::Trajectory::TPoint> tpoints ;
00280 
00281     trajectory::Trajectory::TPoint cur_point(1) ;
00282 
00283     cur_point.dimension_ = 1 ;
00284 
00285     cur_point.q_[0] = low_pt ;
00286     cur_point.time_ = 0.0 ;
00287     tpoints.push_back(cur_point) ;
00288 
00289     cur_point.q_[0] = high_pt ;
00290     cur_point.time_ = cmd.period/2.0 ;
00291     tpoints.push_back(cur_point) ;
00292 
00293     cur_point.q_[0] = low_pt ;
00294     cur_point.time_ = cmd.period ;
00295     tpoints.push_back(cur_point) ;
00296 
00297     if (!setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile)){
00298       ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
00299       return false;
00300     }
00301     else{
00302       ROS_INFO("LaserScannerTrajController: Periodic Command set. Duration=%.4f sec", getProfileDuration()) ;
00303       return true;
00304     }
00305   }
00306   else
00307   {
00308     ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
00309     return false;
00310   }
00311 }
00312 
00313 bool LaserScannerTrajController::setTrajCmd(const pr2_msgs::LaserTrajCmd& traj_cmd)
00314 {
00315   if (traj_cmd.profile == "linear" ||
00316       traj_cmd.profile == "blended_linear")
00317   {
00318     const unsigned int N = traj_cmd.position.size() ;
00319     if (traj_cmd.time_from_start.size() != N)
00320     {
00321       ROS_ERROR("# Times and # Pos must match! pos.size()=%u times.size()=%zu", N, traj_cmd.time_from_start.size()) ;
00322       return false ;
00323     }
00324 
00325     // Load up the trajectory data points, 1 point at a time
00326     std::vector<trajectory::Trajectory::TPoint> tpoints ;
00327     for (unsigned int i=0; i<N; i++)
00328     {
00329       trajectory::Trajectory::TPoint cur_point(1) ;
00330       cur_point.dimension_ = 1 ;
00331       cur_point.q_[0] = traj_cmd.position[i] ;
00332       cur_point.time_ = traj_cmd.time_from_start[i].toSec() ;
00333       tpoints.push_back(cur_point) ;
00334     }
00335 
00336     double cur_max_rate = max_rate_ ;
00337     double cur_max_acc  = max_acc_ ;
00338 
00339     // Overwrite our limits, if they're specified in the msg. Is this maybe too dangerous?
00340     if (traj_cmd.max_velocity > 0)                  // Only overwrite if a positive val
00341       cur_max_rate = traj_cmd.max_velocity ;
00342     if (traj_cmd.max_acceleration > 0)
00343       cur_max_acc = traj_cmd.max_acceleration ;
00344 
00345     if (!setTrajectory(tpoints, cur_max_rate, cur_max_acc, traj_cmd.profile))
00346     {
00347       ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
00348       return false;
00349     }
00350     else
00351     {
00352       ROS_INFO("LaserScannerTrajController: Trajectory Command set. Duration=%.4f sec", getProfileDuration()) ;
00353       return true;
00354     }
00355   }
00356   else
00357   {
00358     ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
00359     return false;
00360   }
00361 }
00362 
00363 /*bool LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
00364 {
00365   while (!track_link_lock_.try_lock())
00366     usleep(100) ;
00367 
00368   if (track_link_cmd.enable)
00369   {
00370     ROS_INFO("LaserScannerTrajController:: Tracking link %s", track_link_cmd.link_name.c_str()) ;
00371     track_link_enabled_ = true ;
00372     string mount_link_name = "laser_tilt_mount_link" ;
00373     target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
00374     mount_link_  = robot_->getLinkState(mount_link_name) ;
00375     tf::pointMsgToTF(track_link_cmd.point, track_point_) ;
00376 
00377     if (target_link_ == NULL)
00378     {
00379       ROS_ERROR("LaserScannerTrajController:: Could not find target link:%s", track_link_cmd.link_name.c_str()) ;
00380       track_link_enabled_ = false ;
00381     }
00382     if (mount_link_ == NULL)
00383     {
00384       ROS_ERROR("LaserScannerTrajController:: Could not find mount link:%s", mount_link_name.c_str()) ;
00385       track_link_enabled_ = false ;
00386     }
00387   }
00388   else
00389   {
00390     track_link_enabled_ = false ;
00391     ROS_INFO("LaserScannerTrajController:: No longer tracking link") ;
00392   }
00393 
00394   track_link_lock_.unlock() ;
00395 
00396   return track_link_enabled_;
00397   }*/
00398 
00399 
00400 LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): c_()
00401 {
00402   prev_profile_segment_ = -1 ;
00403   need_to_send_msg_ = false ;                                           // Haven't completed a sweep yet, so don't need to send a msg
00404   publisher_ = NULL ;                                                   // We don't know our topic yet, so we can't build it
00405 }
00406 
00407 LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
00408 {
00409   if (publisher_)
00410   {
00411     publisher_->stop();
00412     delete publisher_;    // Probably should wait on publish_->is_running() before exiting. Need to
00413                           //   look into shutdown semantics for realtime_publisher
00414   }
00415 }
00416 
00417 void LaserScannerTrajControllerNode::update()
00418 {
00419   c_.update() ;
00420 
00421   int cur_profile_segment = c_.getCurProfileSegment() ;
00422 
00423   if (cur_profile_segment != prev_profile_segment_)
00424   {
00425     // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
00426     ros::Time cur_time(robot_->getTime()) ;
00427     m_scanner_signal_.header.stamp = cur_time ;
00428     m_scanner_signal_.signal = cur_profile_segment ;
00429     need_to_send_msg_ = true ;
00430   }
00431 
00432   prev_profile_segment_ = cur_profile_segment ;
00433 
00434   // Use the realtime_publisher to try to send the message.
00435   //   If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
00436   if (need_to_send_msg_)
00437   {
00438     if (publisher_->trylock())
00439     {
00440       publisher_->msg_.header = m_scanner_signal_.header ;
00441       publisher_->msg_.signal = m_scanner_signal_.signal ;
00442       publisher_->unlockAndPublish() ;
00443       need_to_send_msg_ = false ;
00444     }
00445   }
00446 }
00447 
00448 bool LaserScannerTrajControllerNode::init(pr2_mechanism_model::RobotState *robot,
00449                                           ros::NodeHandle &n)
00450 {
00451   robot_ = robot ;      // Need robot in order to grab hardware time
00452 
00453   node_ = n;
00454 
00455   if (!c_.init(robot, n))
00456   {
00457     ROS_ERROR("Error Loading LaserScannerTrajController Params") ;
00458     return false ;
00459   }
00460 
00461   sub_set_periodic_cmd_ =
00462     node_.subscribe("set_periodic_cmd", 1, &LaserScannerTrajControllerNode::setPeriodicCmd, this) ;
00463   sub_set_traj_cmd_ =
00464     node_.subscribe("set_traj_cmd", 1, &LaserScannerTrajControllerNode::setTrajCmd, this) ;
00465 
00466   serve_set_periodic_cmd_ =
00467     node_.advertiseService("set_periodic_cmd", &LaserScannerTrajControllerNode::setPeriodicSrv, this);
00468   serve_set_Traj_cmd_ =
00469     node_.advertiseService("set_traj_cmd", &LaserScannerTrajControllerNode::setTrajSrv, this);
00470 
00471   if (publisher_ != NULL)               // Make sure that we don't memory leak
00472   {
00473     ROS_ERROR("LaserScannerTrajController shouldn't ever execute this line... could be a bug elsewhere");
00474     delete publisher_;
00475   }
00476   publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (node_, "laser_scanner_signal", 1) ;
00477 
00478   prev_profile_segment_ = -1 ;
00479 
00480   ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
00481 
00482   return true ;
00483 }
00484 
00485 
00486 bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_msgs::SetPeriodicCmd::Request &req,
00487                                                     pr2_msgs::SetPeriodicCmd::Response &res)
00488 {
00489   ROS_INFO("LaserScannerTrajControllerNode: set periodic command");
00490 
00491   if (!c_.setPeriodicCmd(req.command))
00492     return false;
00493   else
00494   {
00495     res.start_time = ros::Time::now();
00496     prev_profile_segment_ = -1 ;
00497     return true;
00498   }
00499 }
00500 
00501 void LaserScannerTrajControllerNode::setPeriodicCmd(const pr2_msgs::PeriodicCmdConstPtr &cmd)
00502 {
00503   c_.setPeriodicCmd(*cmd) ;
00504   prev_profile_segment_ = -1 ;
00505 }
00506 
00507 bool LaserScannerTrajControllerNode::setTrajSrv(pr2_msgs::SetLaserTrajCmd::Request &req,
00508                                                 pr2_msgs::SetLaserTrajCmd::Response &res)
00509 {
00510   ROS_INFO("LaserScannerTrajControllerNode: set traj command");
00511 
00512   if (!c_.setTrajCmd(req.command))
00513     return false;
00514   else
00515   {
00516     res.start_time = ros::Time::now();
00517     prev_profile_segment_ = -1 ;
00518     return true;
00519   }
00520 }
00521 
00522 void LaserScannerTrajControllerNode::setTrajCmd(const pr2_msgs::LaserTrajCmdConstPtr &traj_cmd)
00523 {
00524   c_.setTrajCmd(*traj_cmd) ;
00525   prev_profile_segment_ = -1 ;
00526 }
00527 
00528 /*void LaserScannerTrajControllerNode::setTrackLinkCmd()
00529 {
00530   c_.setTrackLinkCmd(track_link_cmd_) ;
00531   }*/
00532 


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52