velocity_smoother_nodelet.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  ** Includes
00011  *****************************************************************************/
00012 
00013 #include <ros/ros.h>
00014 #include <nodelet/nodelet.h>
00015 #include <pluginlib/class_list_macros.h>
00016 
00017 #include <dynamic_reconfigure/server.h>
00018 #include <yocs_velocity_smoother/paramsConfig.h>
00019 
00020 #include <ecl/threads/thread.hpp>
00021 
00022 #include "yocs_velocity_smoother/velocity_smoother_nodelet.hpp"
00023 
00024 /*****************************************************************************
00025  ** Preprocessing
00026  *****************************************************************************/
00027 
00028 #define PERIOD_RECORD_SIZE    5
00029 #define ZERO_VEL_COMMAND      geometry_msgs::Twist();
00030 #define IS_ZERO_VEOCITY(a)   ((a.linear.x == 0.0) && (a.angular.z == 0.0))
00031 
00032 /*****************************************************************************
00033 ** Namespaces
00034 *****************************************************************************/
00035 
00036 namespace yocs_velocity_smoother {
00037 
00038 /*********************
00039 ** Implementation
00040 **********************/
00041 
00042 void VelocitySmoother::reconfigCB(yocs_velocity_smoother::paramsConfig &config, uint32_t level)
00043 {
00044   ROS_INFO("Reconfigure request : %f %f %f %f %f",
00045            config.speed_lim_v, config.speed_lim_w, config.accel_lim_v, config.accel_lim_w, config.decel_factor);
00046 
00047   speed_lim_v  = config.speed_lim_v;
00048   speed_lim_w  = config.speed_lim_w;
00049   accel_lim_v  = config.accel_lim_v;
00050   accel_lim_w  = config.accel_lim_w;
00051   decel_factor = config.decel_factor;
00052   decel_lim_v  = decel_factor*accel_lim_v;
00053   decel_lim_w  = decel_factor*accel_lim_w;
00054 }
00055 
00056 void VelocitySmoother::velocityCB(const geometry_msgs::Twist::ConstPtr& msg)
00057 {
00058   // Estimate commands frequency; we do continuously as it can be very different depending on the
00059   // publisher type, and we don't want to impose extra constraints to keep this package flexible
00060   if (period_record.size() < PERIOD_RECORD_SIZE)
00061   {
00062     period_record.push_back((ros::Time::now() - last_cb_time).toSec());
00063   }
00064   else
00065   {
00066     period_record[pr_next] = (ros::Time::now() - last_cb_time).toSec();
00067   }
00068 
00069   pr_next++;
00070   pr_next %= period_record.size();
00071   last_cb_time = ros::Time::now();
00072 
00073   if (period_record.size() <= PERIOD_RECORD_SIZE/2)
00074   {
00075     // wait until we have some values; make a reasonable assumption (10 Hz) meanwhile
00076     cb_avg_time = 0.1;
00077   }
00078   else
00079   {
00080     // enough; recalculate with the latest input
00081     cb_avg_time = median(period_record);
00082   }
00083 
00084   input_active = true;
00085 
00086   // Bound speed with the maximum values
00087   target_vel.linear.x  =
00088       msg->linear.x  > 0.0 ? std::min(msg->linear.x,  speed_lim_v) : std::max(msg->linear.x,  -speed_lim_v);
00089   target_vel.angular.z =
00090       msg->angular.z > 0.0 ? std::min(msg->angular.z, speed_lim_w) : std::max(msg->angular.z, -speed_lim_w);
00091 }
00092 
00093 void VelocitySmoother::odometryCB(const nav_msgs::Odometry::ConstPtr& msg)
00094 {
00095   if (robot_feedback == ODOMETRY)
00096     current_vel = msg->twist.twist;
00097 
00098   // ignore otherwise
00099 }
00100 
00101 void VelocitySmoother::robotVelCB(const geometry_msgs::Twist::ConstPtr& msg)
00102 {
00103   if (robot_feedback == COMMANDS)
00104     current_vel = *msg;
00105 
00106   // ignore otherwise
00107 }
00108 
00109 void VelocitySmoother::spin()
00110 {
00111   double period = 1.0/frequency;
00112   ros::Rate spin_rate(frequency);
00113 
00114   while (! shutdown_req && ros::ok())
00115   {
00116     if ((input_active == true) && (cb_avg_time > 0.0) &&
00117         ((ros::Time::now() - last_cb_time).toSec() > std::min(3.0*cb_avg_time, 0.5)))
00118     {
00119       // Velocity input no active anymore; normally last command is a zero-velocity one, but reassure
00120       // this, just in case something went wrong with our input, or he just forgot good manners...
00121       // Issue #2, extra check in case cb_avg_time is very big, for example with several atomic commands
00122       // The cb_avg_time > 0 check is required to deal with low-rate simulated time, that can make that
00123       // several messages arrive with the same time and so lead to a zero median
00124       input_active = false;
00125       if (IS_ZERO_VEOCITY(target_vel) == false)
00126       {
00127         ROS_WARN_STREAM("Velocity Smoother : input got inactive leaving us a non-zero target velocity ("
00128               << target_vel.linear.x << ", " << target_vel.angular.z << "), zeroing...[" << name << "]");
00129         target_vel = ZERO_VEL_COMMAND;
00130       }
00131     }
00132 
00133     if ((robot_feedback != NONE) && (input_active == true) && (cb_avg_time > 0.0) &&
00134         (((ros::Time::now() - last_cb_time).toSec() > 5.0*cb_avg_time)     || // 5 missing msgs
00135           (std::abs(current_vel.linear.x  - last_cmd_vel.linear.x)  > 0.2) ||
00136           (std::abs(current_vel.angular.z - last_cmd_vel.angular.z) > 2.0)))
00137     {
00138       // If the publisher has been inactive for a while, or if our current commanding differs a lot
00139       // from robot velocity feedback, we cannot trust the former; relay on robot's feedback instead
00140       // This can happen mainly due to preemption of current controller on velocity multiplexer.
00141       // TODO: current command/feedback difference thresholds are 진짜 arbitrary; they should somehow
00142       // be proportional to max v and w...
00143       // The one for angular velocity is very big because is it's less necessary (for example the
00144       // reactive controller will never make the robot spin) and because the gyro has a 15 ms delay
00145       ROS_WARN("Using robot velocity feedback (%s) instead of last command: %f, %f, %f",
00146                 robot_feedback == ODOMETRY ? "odometry" : "end commands",
00147                (ros::Time::now()      - last_cb_time).toSec(),
00148                 current_vel.linear.x  - last_cmd_vel.linear.x,
00149                 current_vel.angular.z - last_cmd_vel.angular.z);
00150       last_cmd_vel = current_vel;
00151     }
00152 
00153     geometry_msgs::TwistPtr cmd_vel;
00154 
00155     if ((target_vel.linear.x  != last_cmd_vel.linear.x) ||
00156         (target_vel.angular.z != last_cmd_vel.angular.z))
00157     {
00158       // Try to reach target velocity ensuring that we don't exceed the acceleration limits
00159       cmd_vel.reset(new geometry_msgs::Twist(target_vel));
00160 
00161       double v_inc, w_inc, max_v_inc, max_w_inc;
00162 
00163       v_inc = target_vel.linear.x - last_cmd_vel.linear.x;
00164       if ((robot_feedback == ODOMETRY) && (current_vel.linear.x*target_vel.linear.x < 0.0))
00165       {
00166         // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
00167         max_v_inc = decel_lim_v*period;
00168       }
00169       else
00170       {
00171         max_v_inc = ((v_inc*target_vel.linear.x > 0.0)?accel_lim_v:decel_lim_v)*period;
00172       }
00173 
00174       w_inc = target_vel.angular.z - last_cmd_vel.angular.z;
00175       if ((robot_feedback == ODOMETRY) && (current_vel.angular.z*target_vel.angular.z < 0.0))
00176       {
00177         // countermarch (on robots with significant inertia; requires odometry feedback to be detected)
00178         max_w_inc = decel_lim_w*period;
00179       }
00180       else
00181       {
00182         max_w_inc = ((w_inc*target_vel.angular.z > 0.0)?accel_lim_w:decel_lim_w)*period;
00183       }
00184 
00185       // Calculate and normalise vectors A (desired velocity increment) and B (maximum velocity increment),
00186       // where v acts as coordinate x and w as coordinate y; the sign of the angle from A to B determines
00187       // which velocity (v or w) must be overconstrained to keep the direction provided as command
00188       double MA = sqrt(    v_inc *     v_inc +     w_inc *     w_inc);
00189       double MB = sqrt(max_v_inc * max_v_inc + max_w_inc * max_w_inc);
00190 
00191       double Av = std::abs(v_inc) / MA;
00192       double Aw = std::abs(w_inc) / MA;
00193       double Bv = max_v_inc / MB;
00194       double Bw = max_w_inc / MB;
00195       double theta = atan2(Bw, Bv) - atan2(Aw, Av);
00196 
00197       if (theta < 0)
00198       {
00199         // overconstrain linear velocity
00200         max_v_inc = (max_w_inc*std::abs(v_inc))/std::abs(w_inc);
00201       }
00202       else
00203       {
00204         // overconstrain angular velocity
00205         max_w_inc = (max_v_inc*std::abs(w_inc))/std::abs(v_inc);
00206       }
00207 
00208       if (std::abs(v_inc) > max_v_inc)
00209       {
00210         // we must limit linear velocity
00211         cmd_vel->linear.x  = last_cmd_vel.linear.x  + sign(v_inc)*max_v_inc;
00212       }
00213 
00214       if (std::abs(w_inc) > max_w_inc)
00215       {
00216         // we must limit angular velocity
00217         cmd_vel->angular.z = last_cmd_vel.angular.z + sign(w_inc)*max_w_inc;
00218       }
00219 
00220       smooth_vel_pub.publish(cmd_vel);
00221       last_cmd_vel = *cmd_vel;
00222     }
00223     else if (input_active == true)
00224     {
00225       // We already reached target velocity; just keep resending last command while input is active
00226       cmd_vel.reset(new geometry_msgs::Twist(last_cmd_vel));
00227       smooth_vel_pub.publish(cmd_vel);
00228     }
00229 
00230     spin_rate.sleep();
00231   }
00232 }
00233 
00239 bool VelocitySmoother::init(ros::NodeHandle& nh)
00240 {
00241   // Dynamic Reconfigure
00242   dynamic_reconfigure_callback = boost::bind(&VelocitySmoother::reconfigCB, this, _1, _2);
00243 
00244   dynamic_reconfigure_server = new dynamic_reconfigure::Server<yocs_velocity_smoother::paramsConfig>(nh);
00245   dynamic_reconfigure_server->setCallback(dynamic_reconfigure_callback);
00246 
00247   // Optional parameters
00248   int feedback;
00249   nh.param("frequency",      frequency,     20.0);
00250   nh.param("decel_factor",   decel_factor,   1.0);
00251   nh.param("robot_feedback", feedback, (int)NONE);
00252 
00253   if ((int(feedback) < NONE) || (int(feedback) > COMMANDS))
00254   {
00255     ROS_WARN("Invalid robot feedback type (%d). Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)",
00256              feedback);
00257     feedback = NONE;
00258   }
00259 
00260   robot_feedback = static_cast<RobotFeedbackType>(feedback);
00261 
00262   // Mandatory parameters
00263   if ((nh.getParam("speed_lim_v", speed_lim_v) == false) ||
00264       (nh.getParam("speed_lim_w", speed_lim_w) == false))
00265   {
00266     ROS_ERROR("Missing velocity limit parameter(s)");
00267     return false;
00268   }
00269 
00270   if ((nh.getParam("accel_lim_v", accel_lim_v) == false) ||
00271       (nh.getParam("accel_lim_w", accel_lim_w) == false))
00272   {
00273     ROS_ERROR("Missing acceleration limit parameter(s)");
00274     return false;
00275   }
00276 
00277   // Deceleration can be more aggressive, if necessary
00278   decel_lim_v = decel_factor*accel_lim_v;
00279   decel_lim_w = decel_factor*accel_lim_w;
00280 
00281   // Publishers and subscribers
00282   odometry_sub    = nh.subscribe("odometry",      1, &VelocitySmoother::odometryCB, this);
00283   current_vel_sub = nh.subscribe("robot_cmd_vel", 1, &VelocitySmoother::robotVelCB, this);
00284   raw_in_vel_sub  = nh.subscribe("raw_cmd_vel",   1, &VelocitySmoother::velocityCB, this);
00285   smooth_vel_pub  = nh.advertise <geometry_msgs::Twist> ("smooth_cmd_vel", 1);
00286 
00287   return true;
00288 }
00289 
00290 
00291 /*********************
00292 ** Nodelet
00293 **********************/
00294 
00295 class VelocitySmootherNodelet : public nodelet::Nodelet
00296 {
00297 public:
00298   VelocitySmootherNodelet()  { }
00299   ~VelocitySmootherNodelet()
00300   {
00301     NODELET_DEBUG("Velocity Smoother : waiting for worker thread to finish...");
00302     vel_smoother_->shutdown();
00303     worker_thread_.join();
00304   }
00305 
00306   std::string unresolvedName(const std::string &name) const {
00307     size_t pos = name.find_last_of('/');
00308     return name.substr(pos + 1);
00309   }
00310 
00311 
00312   virtual void onInit()
00313   {
00314     ros::NodeHandle ph = getPrivateNodeHandle();
00315     std::string resolved_name = ph.getUnresolvedNamespace(); // this always returns like /robosem/goo_arm - why not unresolved?
00316     std::string name = unresolvedName(resolved_name); // unresolve it ourselves
00317     NODELET_DEBUG_STREAM("Velocity Smoother : initialising nodelet...[" << name << "]");
00318     vel_smoother_.reset(new VelocitySmoother(name));
00319     if (vel_smoother_->init(ph))
00320     {
00321       NODELET_DEBUG_STREAM("Velocity Smoother : nodelet initialised [" << name << "]");
00322       worker_thread_.start(&VelocitySmoother::spin, *vel_smoother_);
00323     }
00324     else
00325     {
00326       NODELET_ERROR_STREAM("Velocity Smoother : nodelet initialisation failed [" << name << "]");
00327     }
00328   }
00329 
00330 private:
00331   boost::shared_ptr<VelocitySmoother> vel_smoother_;
00332   ecl::Thread                        worker_thread_;
00333 };
00334 
00335 } // namespace yocs_velocity_smoother
00336 
00337 PLUGINLIB_EXPORT_CLASS(yocs_velocity_smoother::VelocitySmootherNodelet, nodelet::Nodelet);


yocs_velocity_smoother
Author(s): Jorge Santos Simon
autogenerated on Fri Aug 28 2015 13:45:00