#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::Time >	getLatestPose (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	run_permitted_client_
	Client for run_permitted 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

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

nh	NodeHandle to handle node
reference_frame	Reference frame for all transformations
frames_to_observe	List of frames to observer

Definition at line 33 of file speed_observer.cpp.

Member Function Documentation

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

speeds Vector containing one speed per observed frame

Returns: The message

Definition at line 162 of file speed_observer.cpp.

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

frame Which frame should be transformed

Returns: The pose as Vector and the time stamp

Definition at line 68 of file speed_observer.cpp.

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

speed The speed to check

Returns: True iff speed is within limit

Definition at line 161 of file speed_observer.h.

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

Callback for service to set the currently active speed limit.

Parameters

req	Service request
res	Service response

Returns: True if the limit was set succesfully

Definition at line 193 of file speed_observer.cpp.

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

a	Pose at the beginning of the trajectory
b	Pose at the end of the trajectory
t	The time the motion took

Returns: The minimal speed for the trajectory

Definition at line 154 of file speed_observer.h.

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_calculations	Number of iterations that can have outdated tf data before a Stop1 is triggered.

Definition at line 79 of file speed_observer.cpp.

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.

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.

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

frame	Which fram should be transformed
time	At what time is the transfomration needed
max_num_retries	How 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

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.

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.

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.

ros::Publisher prbt_hardware_support::SpeedObserver::frame_speeds_pub_

private

Publisher for frame speed message.

Definition at line 122 of file speed_observer.h.

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.

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.

ros::NodeHandle prbt_hardware_support::SpeedObserver::nh_

private

Definition at line 120 of file speed_observer.h.

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

private

Reference frame for all speeds.

Definition at line 131 of file speed_observer.h.

ros::ServiceClient prbt_hardware_support::SpeedObserver::run_permitted_client_

private

Client for run_permitted service.

Definition at line 124 of file speed_observer.h.

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.

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.

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.

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.

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:

Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation