Public Member Functions | Private Member Functions | Static Private Member Functions | Private Attributes | Static Private Attributes | List of all members
prbt_hardware_support::SpeedObserver Class Reference

#include <speed_observer.h>

Public Member Functions

bool setSpeedLimitCb (SetSpeedLimit::Request &req, SetSpeedLimit::Response &res)
 Callback for service to set the currently active speed limit. More...
 
 SpeedObserver (ros::NodeHandle &nh, std::string &reference_frame, std::vector< std::string > &frames_to_observe)
 Create an observer to observe the speed of a list of frames in reference to a reference frame. More...
 
void startObserving (const double frequency, const unsigned int allowed_missed_calculations=DEFAULT_ALLOWED_MISSED_CALCULATIONS)
 Starts the observation cycle. The function blocks until ros shuts down. More...
 
void terminateNow ()
 This method will terminate the observation cycle started by startObserving(). More...
 

Private Member Functions

FrameSpeeds createFrameSpeedsMessage (const std::map< std::string, double > &speeds) const
 Creates a message to be sent to the 'frame_speeds' topic. More...
 
std::pair< tf2::Vector3, ros::TimegetLatestPose (const std::string &frame) const
 Using tf to get the latest Pose of a frame as tf::Vector3. More...
 
bool isWithinLimit (const double &speed) const
 Check if a speed value is within the currently set limit. More...
 
void triggerStop1 ()
 Used to trigger stop of the robot when speed limit is exceeded. More...
 
void waitUntillCanTransform (const std::string &frame, const ros::Time &time, const unsigned short int max_num_retries=10) const
 Helper method waiting until TF transformation is available. More...
 

Static Private Member Functions

static double speedFromTwoPoses (const tf2::Vector3 &a, const tf2::Vector3 &b, const double &t)
 Calculate the minimal speed that a trajectory between two poses had if the motion was performed within a given time. More...
 

Private Attributes

double current_speed_limit_ { DEFAULT_SPEED_LIMIT }
 Currently active speed limit. More...
 
ros::Publisher frame_speeds_pub_
 Publisher for frame speed message. More...
 
const std::vector< std::string > frames_to_observe_
 All frames to observe. More...
 
std::map< std::string, unsigned int > missed_calculations_
 Map to store the number of successive missed frame transform calculations. More...
 
ros::NodeHandle nh_
 
const std::string reference_frame_
 Reference frame for all speeds. More...
 
ros::ServiceClient sto_client_
 Client for sto service. More...
 
std::atomic_bool terminate_ { false }
 Helper variable to stop observation cycle. More...
 
tf2_ros::Buffer tf_buffer_
 Needed to receive tf2 transformations over the wire, see https://wiki.ros.org/tf2/Tutorials/. More...
 
tf2_ros::TransformListener tf_listener { tf_buffer_ }
 Listing to TF Transforms. More...
 

Static Private Attributes

static constexpr uint32_t DEFAULT_QUEUE_SIZE { 10 }
 Default queue size for publisher. More...
 
static constexpr double DEFAULT_SPEED_LIMIT { .25 }
 Speed limit to be set at launch. More...
 
static constexpr double TIME_INTERVAL_EPSILON_S { 1e-9 }
 Epsilon prevents computation of speed for small time intervals. More...
 
static constexpr double WAITING_TIME_FOR_TRANSFORM_S { 0.1 }
 Waiting time for waitUntillCanTransform() More...
 

Detailed Description

Definition at line 37 of file speed_observer.h.

Constructor & Destructor Documentation

◆ SpeedObserver()

prbt_hardware_support::SpeedObserver::SpeedObserver ( ros::NodeHandle nh,
std::string &  reference_frame,
std::vector< std::string > &  frames_to_observe 
)

Create an observer to observe the speed of a list of frames in reference to a reference frame.

Parameters
nhNodeHandle to handle node
reference_frameReference frame for all transformations
frames_to_observeList of frames to observer

Definition at line 33 of file speed_observer.cpp.

Member Function Documentation

◆ createFrameSpeedsMessage()

FrameSpeeds prbt_hardware_support::SpeedObserver::createFrameSpeedsMessage ( const std::map< std::string, double > &  speeds) const
private

Creates a message to be sent to the 'frame_speeds' topic.

Parameters
speedsVector containing one speed per observed frame
Returns
The message

Definition at line 162 of file speed_observer.cpp.

◆ getLatestPose()

std::pair< tf2::Vector3, ros::Time > prbt_hardware_support::SpeedObserver::getLatestPose ( const std::string &  frame) const
private

Using tf to get the latest Pose of a frame as tf::Vector3.

Note
Reference frame is always reference_frame_
Parameters
frameWhich frame should be transformed
Returns
The pose as Vector and the time stamp

Definition at line 68 of file speed_observer.cpp.

◆ isWithinLimit()

bool prbt_hardware_support::SpeedObserver::isWithinLimit ( const double &  speed) const
inlineprivate

Check if a speed value is within the currently set limit.

Note
Needed for unittest
Parameters
speedThe speed to check
Returns
True iff speed is within limit

Definition at line 161 of file speed_observer.h.

◆ setSpeedLimitCb()

bool prbt_hardware_support::SpeedObserver::setSpeedLimitCb ( SetSpeedLimit::Request &  req,
SetSpeedLimit::Response &  res 
)

Callback for service to set the currently active speed limit.

Parameters
reqService request
resService response
Returns
True if the limit was set succesfully

Definition at line 193 of file speed_observer.cpp.

◆ speedFromTwoPoses()

double prbt_hardware_support::SpeedObserver::speedFromTwoPoses ( const tf2::Vector3 &  a,
const tf2::Vector3 &  b,
const double &  t 
)
inlinestaticprivate

Calculate the minimal speed that a trajectory between two poses had if the motion was performed within a given time.

Parameters
aPose at the beginning of the trajectory
bPose at the end of the trajectory
tThe time the motion took
Returns
The minimal speed for the trajectory

Definition at line 154 of file speed_observer.h.

◆ startObserving()

void prbt_hardware_support::SpeedObserver::startObserving ( const double  frequency,
const unsigned int  allowed_missed_calculations = DEFAULT_ALLOWED_MISSED_CALCULATIONS 
)

Starts the observation cycle. The function blocks until ros shuts down.

Parameters
frequency[Hz] Will check all frame once per cycle
allowed_missed_calculationsNumber of iterations that can have outdated tf data before a Stop1 is triggered.

Definition at line 79 of file speed_observer.cpp.

◆ terminateNow()

void prbt_hardware_support::SpeedObserver::terminateNow ( )
inline

This method will terminate the observation cycle started by startObserving().

Definition at line 166 of file speed_observer.h.

◆ triggerStop1()

void prbt_hardware_support::SpeedObserver::triggerStop1 ( )
private

Used to trigger stop of the robot when speed limit is exceeded.

Definition at line 177 of file speed_observer.cpp.

◆ waitUntillCanTransform()

void prbt_hardware_support::SpeedObserver::waitUntillCanTransform ( const std::string &  frame,
const ros::Time time,
const unsigned short int  max_num_retries = 10 
) const
private

Helper method waiting until TF transformation is available.

Note
Refernce frame is always reference_frame_
Parameters
frameWhich fram should be transformed
timeAt what time is the transfomration needed
max_num_retriesHow many times should the method wait for WAITING_TIME_FOR_TRANSFORM_S seconds

Definition at line 42 of file speed_observer.cpp.

Member Data Documentation

◆ current_speed_limit_

double prbt_hardware_support::SpeedObserver::current_speed_limit_ { DEFAULT_SPEED_LIMIT }
private

Currently active speed limit.

Definition at line 137 of file speed_observer.h.

◆ DEFAULT_QUEUE_SIZE

constexpr uint32_t prbt_hardware_support::SpeedObserver::DEFAULT_QUEUE_SIZE { 10 }
staticprivate

Default queue size for publisher.

Definition at line 145 of file speed_observer.h.

◆ DEFAULT_SPEED_LIMIT

constexpr double prbt_hardware_support::SpeedObserver::DEFAULT_SPEED_LIMIT { .25 }
staticprivate

Speed limit to be set at launch.

Definition at line 143 of file speed_observer.h.

◆ frame_speeds_pub_

ros::Publisher prbt_hardware_support::SpeedObserver::frame_speeds_pub_
private

Publisher for frame speed message.

Definition at line 122 of file speed_observer.h.

◆ frames_to_observe_

const std::vector<std::string> prbt_hardware_support::SpeedObserver::frames_to_observe_
private

All frames to observe.

Definition at line 133 of file speed_observer.h.

◆ missed_calculations_

std::map<std::string, unsigned int> prbt_hardware_support::SpeedObserver::missed_calculations_
private

Map to store the number of successive missed frame transform calculations.

Definition at line 139 of file speed_observer.h.

◆ nh_

ros::NodeHandle prbt_hardware_support::SpeedObserver::nh_
private

Definition at line 120 of file speed_observer.h.

◆ reference_frame_

const std::string prbt_hardware_support::SpeedObserver::reference_frame_
private

Reference frame for all speeds.

Definition at line 131 of file speed_observer.h.

◆ sto_client_

ros::ServiceClient prbt_hardware_support::SpeedObserver::sto_client_
private

Client for sto service.

Definition at line 124 of file speed_observer.h.

◆ terminate_

std::atomic_bool prbt_hardware_support::SpeedObserver::terminate_ { false }
private

Helper variable to stop observation cycle.

Definition at line 135 of file speed_observer.h.

◆ tf_buffer_

tf2_ros::Buffer prbt_hardware_support::SpeedObserver::tf_buffer_
private

Needed to receive tf2 transformations over the wire, see https://wiki.ros.org/tf2/Tutorials/.

Definition at line 126 of file speed_observer.h.

◆ tf_listener

tf2_ros::TransformListener prbt_hardware_support::SpeedObserver::tf_listener { tf_buffer_ }
private

Listing to TF Transforms.

Definition at line 128 of file speed_observer.h.

◆ TIME_INTERVAL_EPSILON_S

constexpr double prbt_hardware_support::SpeedObserver::TIME_INTERVAL_EPSILON_S { 1e-9 }
staticprivate

Epsilon prevents computation of speed for small time intervals.

Definition at line 149 of file speed_observer.h.

◆ WAITING_TIME_FOR_TRANSFORM_S

constexpr double prbt_hardware_support::SpeedObserver::WAITING_TIME_FOR_TRANSFORM_S { 0.1 }
staticprivate

Waiting time for waitUntillCanTransform()

Definition at line 147 of file speed_observer.h.


The documentation for this class was generated from the following files:


prbt_hardware_support
Author(s):
autogenerated on Thu Feb 13 2020 03:16:52