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(cmd, joint_state_->position_,
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_.updatePid(error, filtered_d_error, dt) ;
00203   last_time_ = time ;
00204   last_error_ = error ;
00205 }
00206 
00207 double LaserScannerTrajController::getCurProfileTime()
00208 {
00209   ros::Time time = robot_->getTime();
00210   double time_from_start = (time - traj_start_time_).toSec();
00211   double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
00212   return mod_time ;
00213 }
00214 
00215 double LaserScannerTrajController::getProfileDuration()
00216 {
00217   return traj_duration_ ;
00218 }
00219 
00220 int LaserScannerTrajController::getCurProfileSegment()
00221 {
00222   double cur_time = getCurProfileTime() ;
00223   return traj_.findTrajectorySegment(cur_time) ;
00224 }
00225 
00226 bool LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
00227 {
00228   while (!traj_lock_.try_lock())
00229     usleep(100) ;
00230 
00231   vector<double> max_rates ;
00232   max_rates.push_back(max_rate) ;
00233   vector<double> max_accs ;
00234   max_accs.push_back(max_acc) ;
00235 
00236 
00237   traj_.autocalc_timing_ = true;
00238 
00239   traj_.setMaxRates(max_rates) ;
00240   traj_.setMaxAcc(max_accs) ;
00241   traj_.setInterpolationMethod(interp) ;
00242 
00243   traj_.setTrajectory(traj_points) ;
00244 
00245   traj_start_time_ = robot_->getTime() ;
00246 
00247   traj_duration_ = traj_.getTotalTime() ;
00248 
00249   traj_lock_.unlock() ;
00250 
00251   return true;
00252 }
00253 
00254 bool LaserScannerTrajController::setPeriodicCmd(const pr2_msgs::PeriodicCmd& cmd)
00255 {
00256   if (cmd.profile == "linear" ||
00257       cmd.profile == "blended_linear")
00258   {
00259     double high_pt = cmd.amplitude + cmd.offset ;
00260     double low_pt = -cmd.amplitude + cmd.offset ;
00261 
00262 
00263     double soft_limit_low  = joint_state_->joint_->limits->lower;
00264     double soft_limit_high = joint_state_->joint_->limits->upper;
00265 
00266     if (low_pt < soft_limit_low)
00267     {
00268       ROS_WARN("Lower setpoint (%.3f) is below the soft limit (%.3f). Truncating command", low_pt, soft_limit_low) ;
00269       low_pt = soft_limit_low ;
00270     }
00271 
00272     if (high_pt > soft_limit_high)
00273     {
00274       ROS_WARN("Upper setpoint (%.3f) is above the soft limit (%.3f). Truncating command", high_pt, soft_limit_high) ;
00275       high_pt = soft_limit_high ;
00276     }
00277 
00278     std::vector<trajectory::Trajectory::TPoint> tpoints ;
00279 
00280     trajectory::Trajectory::TPoint cur_point(1) ;
00281 
00282     cur_point.dimension_ = 1 ;
00283 
00284     cur_point.q_[0] = low_pt ;
00285     cur_point.time_ = 0.0 ;
00286     tpoints.push_back(cur_point) ;
00287 
00288     cur_point.q_[0] = high_pt ;
00289     cur_point.time_ = cmd.period/2.0 ;
00290     tpoints.push_back(cur_point) ;
00291 
00292     cur_point.q_[0] = low_pt ;
00293     cur_point.time_ = cmd.period ;
00294     tpoints.push_back(cur_point) ;
00295 
00296     if (!setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile)){
00297       ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
00298       return false;
00299     }
00300     else{
00301       ROS_INFO("LaserScannerTrajController: Periodic Command set. Duration=%.4f sec", getProfileDuration()) ;
00302       return true;
00303     }
00304   }
00305   else
00306   {
00307     ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
00308     return false;
00309   }
00310 }
00311 
00312 bool LaserScannerTrajController::setTrajCmd(const pr2_msgs::LaserTrajCmd& traj_cmd)
00313 {
00314   if (traj_cmd.profile == "linear" ||
00315       traj_cmd.profile == "blended_linear")
00316   {
00317     const unsigned int N = traj_cmd.position.size() ;
00318     if (traj_cmd.time_from_start.size() != N)
00319     {
00320       ROS_ERROR("# Times and # Pos must match! pos.size()=%u times.size()=%u", N, traj_cmd.time_from_start.size()) ;
00321       return false ;
00322     }
00323 
00324     // Load up the trajectory data points, 1 point at a time
00325     std::vector<trajectory::Trajectory::TPoint> tpoints ;
00326     for (unsigned int i=0; i<N; i++)
00327     {
00328       trajectory::Trajectory::TPoint cur_point(1) ;
00329       cur_point.dimension_ = 1 ;
00330       cur_point.q_[0] = traj_cmd.position[i] ;
00331       cur_point.time_ = traj_cmd.time_from_start[i].toSec() ;
00332       tpoints.push_back(cur_point) ;
00333     }
00334 
00335     double cur_max_rate = max_rate_ ;
00336     double cur_max_acc  = max_acc_ ;
00337 
00338     // Overwrite our limits, if they're specified in the msg. Is this maybe too dangerous?
00339     if (traj_cmd.max_velocity > 0)                  // Only overwrite if a positive val
00340       cur_max_rate = traj_cmd.max_velocity ;
00341     if (traj_cmd.max_acceleration > 0)
00342       cur_max_acc = traj_cmd.max_acceleration ;
00343 
00344     if (!setTrajectory(tpoints, cur_max_rate, cur_max_acc, traj_cmd.profile))
00345     {
00346       ROS_ERROR("Failed to set tilt laser scanner trajectory.") ;
00347       return false;
00348     }
00349     else
00350     {
00351       ROS_INFO("LaserScannerTrajController: Trajectory Command set. Duration=%.4f sec", getProfileDuration()) ;
00352       return true;
00353     }
00354   }
00355   else
00356   {
00357     ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
00358     return false;
00359   }
00360 }
00361 
00362 /*bool LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
00363 {
00364   while (!track_link_lock_.try_lock())
00365     usleep(100) ;
00366 
00367   if (track_link_cmd.enable)
00368   {
00369     ROS_INFO("LaserScannerTrajController:: Tracking link %s", track_link_cmd.link_name.c_str()) ;
00370     track_link_enabled_ = true ;
00371     string mount_link_name = "laser_tilt_mount_link" ;
00372     target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
00373     mount_link_  = robot_->getLinkState(mount_link_name) ;
00374     tf::pointMsgToTF(track_link_cmd.point, track_point_) ;
00375 
00376     if (target_link_ == NULL)
00377     {
00378       ROS_ERROR("LaserScannerTrajController:: Could not find target link:%s", track_link_cmd.link_name.c_str()) ;
00379       track_link_enabled_ = false ;
00380     }
00381     if (mount_link_ == NULL)
00382     {
00383       ROS_ERROR("LaserScannerTrajController:: Could not find mount link:%s", mount_link_name.c_str()) ;
00384       track_link_enabled_ = false ;
00385     }
00386   }
00387   else
00388   {
00389     track_link_enabled_ = false ;
00390     ROS_INFO("LaserScannerTrajController:: No longer tracking link") ;
00391   }
00392 
00393   track_link_lock_.unlock() ;
00394 
00395   return track_link_enabled_;
00396   }*/
00397 
00398 
00399 LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): c_()
00400 {
00401   prev_profile_segment_ = -1 ;
00402   need_to_send_msg_ = false ;                                           // Haven't completed a sweep yet, so don't need to send a msg
00403   publisher_ = NULL ;                                                   // We don't know our topic yet, so we can't build it
00404 }
00405 
00406 LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
00407 {
00408   if (publisher_)
00409   {
00410     publisher_->stop();
00411     delete publisher_;    // Probably should wait on publish_->is_running() before exiting. Need to
00412                           //   look into shutdown semantics for realtime_publisher
00413   }
00414 }
00415 
00416 void LaserScannerTrajControllerNode::update()
00417 {
00418   c_.update() ;
00419 
00420   int cur_profile_segment = c_.getCurProfileSegment() ;
00421 
00422   if (cur_profile_segment != prev_profile_segment_)
00423   {
00424     // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
00425     ros::Time cur_time(robot_->getTime()) ;
00426     m_scanner_signal_.header.stamp = cur_time ;
00427     m_scanner_signal_.signal = cur_profile_segment ;
00428     need_to_send_msg_ = true ;
00429   }
00430 
00431   prev_profile_segment_ = cur_profile_segment ;
00432 
00433   // Use the realtime_publisher to try to send the message.
00434   //   If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
00435   if (need_to_send_msg_)
00436   {
00437     if (publisher_->trylock())
00438     {
00439       publisher_->msg_.header = m_scanner_signal_.header ;
00440       publisher_->msg_.signal = m_scanner_signal_.signal ;
00441       publisher_->unlockAndPublish() ;
00442       need_to_send_msg_ = false ;
00443     }
00444   }
00445 }
00446 
00447 bool LaserScannerTrajControllerNode::init(pr2_mechanism_model::RobotState *robot,
00448                                           ros::NodeHandle &n)
00449 {
00450   robot_ = robot ;      // Need robot in order to grab hardware time
00451 
00452   node_ = n;
00453 
00454   if (!c_.init(robot, n))
00455   {
00456     ROS_ERROR("Error Loading LaserScannerTrajController Params") ;
00457     return false ;
00458   }
00459 
00460   sub_set_periodic_cmd_ =
00461     node_.subscribe("set_periodic_cmd", 1, &LaserScannerTrajControllerNode::setPeriodicCmd, this) ;
00462   sub_set_traj_cmd_ =
00463     node_.subscribe("set_traj_cmd", 1, &LaserScannerTrajControllerNode::setTrajCmd, this) ;
00464 
00465   serve_set_periodic_cmd_ =
00466     node_.advertiseService("set_periodic_cmd", &LaserScannerTrajControllerNode::setPeriodicSrv, this);
00467   serve_set_Traj_cmd_ =
00468     node_.advertiseService("set_traj_cmd", &LaserScannerTrajControllerNode::setTrajSrv, this);
00469 
00470   if (publisher_ != NULL)               // Make sure that we don't memory leak
00471   {
00472     ROS_ERROR("LaserScannerTrajController shouldn't ever execute this line... could be a bug elsewhere");
00473     delete publisher_;
00474   }
00475   publisher_ = new realtime_tools::RealtimePublisher <pr2_msgs::LaserScannerSignal> (node_, "laser_scanner_signal", 1) ;
00476 
00477   prev_profile_segment_ = -1 ;
00478 
00479   ROS_INFO("Successfully spawned %s", service_prefix_.c_str()) ;
00480 
00481   return true ;
00482 }
00483 
00484 
00485 bool LaserScannerTrajControllerNode::setPeriodicSrv(pr2_msgs::SetPeriodicCmd::Request &req,
00486                                                     pr2_msgs::SetPeriodicCmd::Response &res)
00487 {
00488   ROS_INFO("LaserScannerTrajControllerNode: set periodic command");
00489 
00490   if (!c_.setPeriodicCmd(req.command))
00491     return false;
00492   else
00493   {
00494     res.start_time = ros::Time::now();
00495     prev_profile_segment_ = -1 ;
00496     return true;
00497   }
00498 }
00499 
00500 void LaserScannerTrajControllerNode::setPeriodicCmd(const pr2_msgs::PeriodicCmdConstPtr &cmd)
00501 {
00502   c_.setPeriodicCmd(*cmd) ;
00503   prev_profile_segment_ = -1 ;
00504 }
00505 
00506 bool LaserScannerTrajControllerNode::setTrajSrv(pr2_msgs::SetLaserTrajCmd::Request &req,
00507                                                 pr2_msgs::SetLaserTrajCmd::Response &res)
00508 {
00509   ROS_INFO("LaserScannerTrajControllerNode: set traj command");
00510 
00511   if (!c_.setTrajCmd(req.command))
00512     return false;
00513   else
00514   {
00515     res.start_time = ros::Time::now();
00516     prev_profile_segment_ = -1 ;
00517     return true;
00518   }
00519 }
00520 
00521 void LaserScannerTrajControllerNode::setTrajCmd(const pr2_msgs::LaserTrajCmdConstPtr &traj_cmd)
00522 {
00523   c_.setTrajCmd(*traj_cmd) ;
00524   prev_profile_segment_ = -1 ;
00525 }
00526 
00527 /*void LaserScannerTrajControllerNode::setTrackLinkCmd()
00528 {
00529   c_.setTrackLinkCmd(track_link_cmd_) ;
00530   }*/
00531 


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Fri Jan 3 2014 11:41:46