#include <velocity_smoother.h>
Public Member Functions | |
| bool | init (ros::NodeHandle &nh) |
| void | shutdown () |
| void | spin () |
| VelocitySmoother (const std::string &name) | |
| ~VelocitySmoother () | |
Private Types | |
| enum | RobotFeedbackType { NONE, ODOMETRY, COMMANDS } |
Private Member Functions | |
| double | median (std::vector< double > values) |
| void | odometryCB (const nav_msgs::Odometry::ConstPtr &msg) |
| void | reconfigCB (cob_base_velocity_smoother::paramsConfig &config, uint32_t unused_level) |
| void | robotVelCB (const geometry_msgs::Twist::ConstPtr &msg) |
| double | sign (double x) |
| void | velocityCB (const geometry_msgs::Twist::ConstPtr &msg) |
Private Attributes | |
| double | accel_lim_vx |
| double | accel_lim_vy |
| double | accel_lim_w |
| double | cb_avg_time |
| geometry_msgs::Twist | current_vel |
| ros::Subscriber | current_vel_sub |
| double | decel_factor |
| double | decel_factor_safe |
| double | decel_lim_vx |
| double | decel_lim_vx_safe |
| double | decel_lim_vy |
| double | decel_lim_vy_safe |
| double | decel_lim_w |
| double | decel_lim_w_safe |
| dynamic_reconfigure::Server < cob_base_velocity_smoother::paramsConfig > ::CallbackType | dynamic_reconfigure_callback |
| dynamic_reconfigure::Server < cob_base_velocity_smoother::paramsConfig > * | dynamic_reconfigure_server |
| double | frequency |
| bool | input_active |
| ros::Time | last_cb_time |
| geometry_msgs::Twist | last_cmd_vel |
| std::string | name |
| ros::Subscriber | odometry_sub |
| std::vector< double > | period_record |
| unsigned int | pr_next |
| ros::Subscriber | raw_in_vel_sub |
| enum cob_base_velocity_smoother::VelocitySmoother::RobotFeedbackType | robot_feedback |
| bool | shutdown_req |
| ros::Publisher | smooth_vel_pub |
| double | speed_lim_vx |
| double | speed_lim_vy |
| double | speed_lim_w |
| geometry_msgs::Twist | target_vel |
Definition at line 40 of file velocity_smoother.h.
Definition at line 56 of file velocity_smoother.h.
| cob_base_velocity_smoother::VelocitySmoother::VelocitySmoother | ( | const std::string & | name | ) |
Definition at line 46 of file velocity_smoother.cpp.
Definition at line 45 of file velocity_smoother.h.
Initialise from a nodelet's private nodehandle.
| nh | : private nodehandle |
Definition at line 300 of file velocity_smoother.cpp.
| double cob_base_velocity_smoother::VelocitySmoother::median | ( | std::vector< double > | values | ) | [inline, private] |
Definition at line 93 of file velocity_smoother.h.
| void cob_base_velocity_smoother::VelocitySmoother::odometryCB | ( | const nav_msgs::Odometry::ConstPtr & | msg | ) | [private] |
Definition at line 115 of file velocity_smoother.cpp.
| void cob_base_velocity_smoother::VelocitySmoother::reconfigCB | ( | cob_base_velocity_smoother::paramsConfig & | config, |
| uint32_t | unused_level | ||
| ) | [private] |
Definition at line 55 of file velocity_smoother.cpp.
| void cob_base_velocity_smoother::VelocitySmoother::robotVelCB | ( | const geometry_msgs::Twist::ConstPtr & | msg | ) | [private] |
Definition at line 123 of file velocity_smoother.cpp.
| void cob_base_velocity_smoother::VelocitySmoother::shutdown | ( | ) | [inline] |
Definition at line 53 of file velocity_smoother.h.
| double cob_base_velocity_smoother::VelocitySmoother::sign | ( | double | x | ) | [inline, private] |
Definition at line 91 of file velocity_smoother.h.
Definition at line 131 of file velocity_smoother.cpp.
| void cob_base_velocity_smoother::VelocitySmoother::velocityCB | ( | const geometry_msgs::Twist::ConstPtr & | msg | ) | [private] |
Definition at line 76 of file velocity_smoother.cpp.
double cob_base_velocity_smoother::VelocitySmoother::accel_lim_vx [private] |
Definition at line 64 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::accel_lim_vy [private] |
Definition at line 65 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::accel_lim_w [private] |
Definition at line 66 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::cb_avg_time [private] |
Definition at line 77 of file velocity_smoother.h.
geometry_msgs::Twist cob_base_velocity_smoother::VelocitySmoother::current_vel [private] |
Definition at line 72 of file velocity_smoother.h.
Current velocity from commands sent to the robot, not necessarily by this node
Definition at line 83 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_factor [private] |
Definition at line 67 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_factor_safe [private] |
Definition at line 67 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_vx [private] |
Definition at line 64 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_vx_safe [private] |
Definition at line 64 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_vy [private] |
Definition at line 65 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_vy_safe [private] |
Definition at line 65 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_w [private] |
Definition at line 66 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::decel_lim_w_safe [private] |
Definition at line 66 of file velocity_smoother.h.
dynamic_reconfigure::Server<cob_base_velocity_smoother::paramsConfig>::CallbackType cob_base_velocity_smoother::VelocitySmoother::dynamic_reconfigure_callback [private] |
Definition at line 103 of file velocity_smoother.h.
dynamic_reconfigure::Server<cob_base_velocity_smoother::paramsConfig>* cob_base_velocity_smoother::VelocitySmoother::dynamic_reconfigure_server [private] |
Definition at line 97 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::frequency [private] |
Definition at line 69 of file velocity_smoother.h.
bool cob_base_velocity_smoother::VelocitySmoother::input_active [private] |
Definition at line 76 of file velocity_smoother.h.
Definition at line 78 of file velocity_smoother.h.
geometry_msgs::Twist cob_base_velocity_smoother::VelocitySmoother::last_cmd_vel [private] |
Definition at line 71 of file velocity_smoother.h.
std::string cob_base_velocity_smoother::VelocitySmoother::name [private] |
Definition at line 63 of file velocity_smoother.h.
Current velocity from odometry
Definition at line 82 of file velocity_smoother.h.
std::vector<double> cob_base_velocity_smoother::VelocitySmoother::period_record [private] |
Historic of latest periods between velocity commands
Definition at line 79 of file velocity_smoother.h.
unsigned int cob_base_velocity_smoother::VelocitySmoother::pr_next [private] |
Next position to fill in the periods record buffer
Definition at line 80 of file velocity_smoother.h.
Incoming raw velocity commands
Definition at line 84 of file velocity_smoother.h.
enum cob_base_velocity_smoother::VelocitySmoother::RobotFeedbackType cob_base_velocity_smoother::VelocitySmoother::robot_feedback [private] |
What source to use as robot velocity feedback
bool cob_base_velocity_smoother::VelocitySmoother::shutdown_req [private] |
Shutdown requested by nodelet; kill worker thread
Definition at line 75 of file velocity_smoother.h.
Outgoing smoothed velocity commands
Definition at line 85 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::speed_lim_vx [private] |
Definition at line 64 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::speed_lim_vy [private] |
Definition at line 65 of file velocity_smoother.h.
double cob_base_velocity_smoother::VelocitySmoother::speed_lim_w [private] |
Definition at line 66 of file velocity_smoother.h.
geometry_msgs::Twist cob_base_velocity_smoother::VelocitySmoother::target_vel [private] |
Definition at line 73 of file velocity_smoother.h.