#include <cob_base_velocity_smoother.h>
Public Member Functions | |
void | calculationStep () |
bool | circBuffOutOfDate (ros::Time now) |
cob_base_velocity_smoother () | |
void | geometryCallback (const geometry_msgs::Twist::ConstPtr &cmd_vel) |
double | getLoopRate () |
bool | IsEqual (geometry_msgs::Twist msg1, geometry_msgs::Twist msg2) |
bool | IsZeroMsg (geometry_msgs::Twist cmd_vel) |
void | limitAcceleration (ros::Time now, geometry_msgs::Twist &cmd_vel) |
double | meanValueX () |
double | meanValueY () |
double | meanValueZ () |
void | reviseCircBuff (ros::Time now, geometry_msgs::Twist cmd_vel) |
geometry_msgs::Twist | setOutput (ros::Time now, geometry_msgs::Twist cmd_vel) |
int | signum (double var) |
~cob_base_velocity_smoother () | |
Public Attributes | |
boost::circular_buffer < geometry_msgs::Twist > | cb_ |
boost::circular_buffer < geometry_msgs::Twist > | cb_out_ |
boost::circular_buffer< ros::Time > | cb_time_ |
ros::Subscriber | geometry_msgs_sub_ |
ros::NodeHandle | nh_ |
ros::NodeHandle | pnh_ |
ros::Publisher | pub_ |
Private Member Functions | |
bool | get_new_msg_received () |
void | set_new_msg_received (bool received) |
Private Attributes | |
double | acc_limit_ |
int | buffer_capacity_ |
double | loop_rate_ |
pthread_mutex_t | m_mutex |
double | max_delay_between_commands_ |
bool | new_msg_received_ |
double | stop_delay_after_no_sub_ |
double | store_delay_ |
geometry_msgs::Twist | sub_msg_ |
geometry_msgs::Twist | zero_values_ |
Definition at line 82 of file cob_base_velocity_smoother.h.
Definition at line 80 of file cob_base_velocity_smoother.cpp.
Definition at line 169 of file cob_base_velocity_smoother.cpp.
Definition at line 181 of file cob_base_velocity_smoother.cpp.
Definition at line 305 of file cob_base_velocity_smoother.cpp.
void cob_base_velocity_smoother::geometryCallback | ( | const geometry_msgs::Twist::ConstPtr & | cmd_vel | ) |
Definition at line 172 of file cob_base_velocity_smoother.cpp.
bool cob_base_velocity_smoother::get_new_msg_received | ( | ) | [private] |
Definition at line 71 of file cob_base_velocity_smoother.cpp.
double cob_base_velocity_smoother::getLoopRate | ( | ) |
Definition at line 472 of file cob_base_velocity_smoother.cpp.
bool cob_base_velocity_smoother::IsEqual | ( | geometry_msgs::Twist | msg1, |
geometry_msgs::Twist | msg2 | ||
) |
Definition at line 479 of file cob_base_velocity_smoother.cpp.
bool cob_base_velocity_smoother::IsZeroMsg | ( | geometry_msgs::Twist | cmd_vel | ) |
Definition at line 325 of file cob_base_velocity_smoother.cpp.
void cob_base_velocity_smoother::limitAcceleration | ( | ros::Time | now, |
geometry_msgs::Twist & | cmd_vel | ||
) |
Definition at line 491 of file cob_base_velocity_smoother.cpp.
double cob_base_velocity_smoother::meanValueX | ( | ) |
Definition at line 346 of file cob_base_velocity_smoother.cpp.
double cob_base_velocity_smoother::meanValueY | ( | ) |
Definition at line 388 of file cob_base_velocity_smoother.cpp.
double cob_base_velocity_smoother::meanValueZ | ( | ) |
Definition at line 430 of file cob_base_velocity_smoother.cpp.
void cob_base_velocity_smoother::reviseCircBuff | ( | ros::Time | now, |
geometry_msgs::Twist | cmd_vel | ||
) |
Definition at line 238 of file cob_base_velocity_smoother.cpp.
void cob_base_velocity_smoother::set_new_msg_received | ( | bool | received | ) | [private] |
Definition at line 64 of file cob_base_velocity_smoother.cpp.
geometry_msgs::Twist cob_base_velocity_smoother::setOutput | ( | ros::Time | now, |
geometry_msgs::Twist | cmd_vel | ||
) |
Definition at line 215 of file cob_base_velocity_smoother.cpp.
int cob_base_velocity_smoother::signum | ( | double | var | ) |
Definition at line 335 of file cob_base_velocity_smoother.cpp.
double cob_base_velocity_smoother::acc_limit_ [private] |
Definition at line 92 of file cob_base_velocity_smoother.h.
int cob_base_velocity_smoother::buffer_capacity_ [private] |
Definition at line 86 of file cob_base_velocity_smoother.h.
boost::circular_buffer<geometry_msgs::Twist> cob_base_velocity_smoother::cb_ |
Definition at line 121 of file cob_base_velocity_smoother.h.
boost::circular_buffer<geometry_msgs::Twist> cob_base_velocity_smoother::cb_out_ |
Definition at line 122 of file cob_base_velocity_smoother.h.
boost::circular_buffer<ros::Time> cob_base_velocity_smoother::cb_time_ |
Definition at line 123 of file cob_base_velocity_smoother.h.
Definition at line 126 of file cob_base_velocity_smoother.h.
double cob_base_velocity_smoother::loop_rate_ [private] |
Definition at line 94 of file cob_base_velocity_smoother.h.
pthread_mutex_t cob_base_velocity_smoother::m_mutex [private] |
Definition at line 102 of file cob_base_velocity_smoother.h.
double cob_base_velocity_smoother::max_delay_between_commands_ [private] |
Definition at line 96 of file cob_base_velocity_smoother.h.
bool cob_base_velocity_smoother::new_msg_received_ [private] |
Definition at line 103 of file cob_base_velocity_smoother.h.
Definition at line 118 of file cob_base_velocity_smoother.h.
Definition at line 118 of file cob_base_velocity_smoother.h.
Definition at line 129 of file cob_base_velocity_smoother.h.
double cob_base_velocity_smoother::stop_delay_after_no_sub_ [private] |
Definition at line 90 of file cob_base_velocity_smoother.h.
double cob_base_velocity_smoother::store_delay_ [private] |
Definition at line 88 of file cob_base_velocity_smoother.h.
geometry_msgs::Twist cob_base_velocity_smoother::sub_msg_ [private] |
Definition at line 100 of file cob_base_velocity_smoother.h.
geometry_msgs::Twist cob_base_velocity_smoother::zero_values_ [private] |
Definition at line 98 of file cob_base_velocity_smoother.h.