#include <diff_drive_controller.h>

Inheritance diagram for diff_drive_controller::DiffDriveController:

Classes
struct	Commands
	Velocity command related: More...

struct	DynamicParams

Public Member Functions
	DiffDriveController ()

bool	init (hardware_interface::VelocityJointInterface *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
	Initialize controller. More...

void	starting (const ros::Time &time)
	Starts controller. More...

void	stopping (const ros::Time &)
	Stops controller. More...

void	update (const ros::Time &time, const ros::Duration &period)
	Updates controller, i.e. computes the odometry and sets the new velocity commands. More...

Public Member Functions inherited from controller_interface::Controller< hardware_interface::VelocityJointInterface >
virtual bool	init (T *, ros::NodeHandle &)

virtual bool	init (T *, ros::NodeHandle &, ros::NodeHandle &)

Public Member Functions inherited from controller_interface::ControllerBase
virtual void	aborting (const ros::Time &)

virtual void	aborting (const ros::Time &)

bool	abortRequest (const ros::Time &time)

bool	abortRequest (const ros::Time &time)

	ControllerBase ()=default

	ControllerBase (const ControllerBase &)=delete

	ControllerBase (ControllerBase &&)=delete

bool	isAborted () const

bool	isAborted () const

bool	isInitialized () const

bool	isInitialized () const

bool	isRunning () const

bool	isRunning () const

bool	isStopped () const

bool	isStopped () const

bool	isWaiting () const

bool	isWaiting () const

ControllerBase &	operator= (const ControllerBase &)=delete

ControllerBase &	operator= (ControllerBase &&)=delete

bool	startRequest (const ros::Time &time)

bool	startRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

virtual void	waiting (const ros::Time &)

virtual void	waiting (const ros::Time &)

bool	waitRequest (const ros::Time &time)

bool	waitRequest (const ros::Time &time)

virtual	~ControllerBase ()=default

Private Types
typedef dynamic_reconfigure::Server< DiffDriveControllerConfig >	ReconfigureServer
	Dynamic Reconfigure server. More...

Private Member Functions
void	brake ()
	Brakes the wheels, i.e. sets the velocity to 0. More...

void	cmdVelCallback (const geometry_msgs::Twist &command)
	Velocity command callback. More...

bool	getWheelNames (ros::NodeHandle &controller_nh, const std::string &wheel_param, std::vector< std::string > &wheel_names)
	Get the wheel names from a wheel param. More...

void	publishWheelData (const ros::Time &time, const ros::Duration &period, Commands &curr_cmd, double wheel_separation, double left_wheel_radius, double right_wheel_radius)

void	reconfCallback (DiffDriveControllerConfig &config, uint32_t)
	Callback for dynamic_reconfigure server. More...

bool	setOdomParamsFromUrdf (ros::NodeHandle &root_nh, const std::string &left_wheel_name, const std::string &right_wheel_name, bool lookup_wheel_separation, bool lookup_wheel_radius)
	Sets odometry parameters from the URDF, i.e. the wheel radius and separation. More...

void	setOdomPubFields (ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
	Sets the odometry publishing fields. More...

void	updateDynamicParams ()
	Update the dynamic parameters in the RT loop. More...

Private Attributes
bool	allow_multiple_cmd_vel_publishers_
	Whether to allow multiple publishers on cmd_vel topic or not: More...

std::string	base_frame_id_
	Frame to use for the robot base: More...

std::shared_ptr< realtime_tools::RealtimePublisher< geometry_msgs::TwistStamped > >	cmd_vel_pub_
	Publish executed commands. More...

double	cmd_vel_timeout_
	Timeout to consider cmd_vel commands old: More...

realtime_tools::RealtimeBuffer< Commands >	command_

Commands	command_struct_

std::shared_ptr< realtime_tools::RealtimePublisher< control_msgs::JointTrajectoryControllerState > >	controller_state_pub_
	Controller state publisher. More...

std::shared_ptr< ReconfigureServer >	dyn_reconf_server_

boost::recursive_mutex	dyn_reconf_server_mutex_

realtime_tools::RealtimeBuffer< DynamicParams >	dynamic_params_

bool	enable_odom_tf_
	Whether to publish odometry to tf or not: More...

Commands	last0_cmd_

Commands	last1_cmd_
	Speed limiters: More...

ros::Time	last_state_publish_time_

std::vector< hardware_interface::JointHandle >	left_wheel_joints_
	Hardware handles: More...

double	left_wheel_radius_multiplier_

SpeedLimiter	limiter_ang_

SpeedLimiter	limiter_lin_

std::string	name_

std::string	odom_frame_id_
	Frame to use for odometry and odom tf: More...

std::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > >	odom_pub_
	Odometry related: More...

Odometry	odometry_

bool	open_loop_

bool	publish_cmd_
	Publish limited velocity: More...

ros::Duration	publish_period_
	Odometry related: More...

bool	publish_wheel_joint_controller_state_
	Publish wheel data: More...

std::vector< hardware_interface::JointHandle >	right_wheel_joints_

double	right_wheel_radius_multiplier_

ros::Subscriber	sub_command_

std::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > >	tf_odom_pub_

ros::Time	time_previous_

double	vel_left_desired_previous_
	Previous velocities from the encoders: More...

std::vector< double >	vel_left_previous_
	Previous velocities from the encoders: More...

double	vel_right_desired_previous_

std::vector< double >	vel_right_previous_

size_t	wheel_joints_size_
	Number of wheel joints: More...

double	wheel_radius_
	Wheel radius (assuming it's the same for the left and right wheels): More...

double	wheel_separation_
	Wheel separation, wrt the midpoint of the wheel width: More...

double	wheel_separation_multiplier_
	Wheel separation and radius calibration multipliers: More...

Additional Inherited Members
Public Types inherited from controller_interface::ControllerBase
typedef std::vector< hardware_interface::InterfaceResources >	ClaimedResources

enum	ControllerState { ControllerState::CONSTRUCTED, ControllerState::INITIALIZED, ControllerState::RUNNING, ControllerState::STOPPED, ControllerState::WAITING, ControllerState::ABORTED }

Public Attributes inherited from controller_interface::ControllerBase
ControllerState	state_

Protected Member Functions inherited from controller_interface::Controller< hardware_interface::VelocityJointInterface >
std::string	getHardwareInterfaceType () const

bool	initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) override

Detailed Description

This class makes some assumptions on the model of the robot:

the rotation axes of wheels are collinear
the wheels are identical in radius Additional assumptions to not duplicate information readily available in the URDF:
the wheels have the same parent frame
a wheel collision geometry is a cylinder or sphere in the urdf
a wheel joint frame center's vertical projection on the floor must lie within the contact patch

Definition at line 99 of file diff_drive_controller.h.

Member Typedef Documentation

◆ ReconfigureServer

typedef dynamic_reconfigure::Server<DiffDriveControllerConfig> diff_drive_controller::DiffDriveController::ReconfigureServer

private

Dynamic Reconfigure server.

Definition at line 299 of file diff_drive_controller.h.

Constructor & Destructor Documentation

◆ DiffDriveController()

diff_drive_controller::DiffDriveController::DiffDriveController ( )

Definition at line 147 of file diff_drive_controller.cpp.

Member Function Documentation

◆ brake()

void diff_drive_controller::DiffDriveController::brake ( )

private

Brakes the wheels, i.e. sets the velocity to 0.

Definition at line 522 of file diff_drive_controller.cpp.

◆ cmdVelCallback()

void diff_drive_controller::DiffDriveController::cmdVelCallback ( const geometry_msgs::Twist & command )

private

Velocity command callback.

Parameters

command Velocity command message (twist)

Definition at line 532 of file diff_drive_controller.cpp.

◆ getWheelNames()

bool diff_drive_controller::DiffDriveController::getWheelNames	(	ros::NodeHandle &	controller_nh,
		const std::string &	wheel_param,
		std::vector< std::string > &	wheel_names
	)

private

Get the wheel names from a wheel param.

Parameters

[in]	controller_nh	Controller node handler
[in]	wheel_param	Param name
[out]	wheel_names	Vector with the whel names

Returns: true if the wheel_param is available and the wheel_names are retrieved successfully from the param server; false otherwise

Definition at line 567 of file diff_drive_controller.cpp.

◆ init()

bool diff_drive_controller::DiffDriveController::init	(	hardware_interface::VelocityJointInterface *	hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)

Initialize controller.

Parameters

hw	Velocity joint interface for the wheels
root_nh	Node handle at root namespace
controller_nh	Node handle inside the controller namespace

Definition at line 166 of file diff_drive_controller.cpp.

◆ publishWheelData()

void diff_drive_controller::DiffDriveController::publishWheelData	(	const ros::Time &	time,
		const ros::Duration &	period,
		Commands &	curr_cmd,
		double	wheel_separation,
		double	left_wheel_radius,
		double	right_wheel_radius
	)

private

Parameters

time	Current time
period	Time since the last called to update
curr_cmd	Current velocity command
wheel_separation	wheel separation with multiplier
left_wheel_radius	left wheel radius with multiplier
right_wheel_radius	right wheel radius with multiplier

Definition at line 765 of file diff_drive_controller.cpp.

◆ reconfCallback()

void diff_drive_controller::DiffDriveController::reconfCallback	(	DiffDriveControllerConfig &	config,
		uint32_t
	)

private

Callback for dynamic_reconfigure server.

Parameters

config	The config set from dynamic_reconfigure server
level	not used at this time.

See also: dyn_reconf_server_

Definition at line 736 of file diff_drive_controller.cpp.

◆ setOdomParamsFromUrdf()

bool diff_drive_controller::DiffDriveController::setOdomParamsFromUrdf	(	ros::NodeHandle &	root_nh,
		const std::string &	left_wheel_name,
		const std::string &	right_wheel_name,
		bool	lookup_wheel_separation,
		bool	lookup_wheel_radius
	)

private

Sets odometry parameters from the URDF, i.e. the wheel radius and separation.

Parameters

root_nh	Root node handle
left_wheel_name	Name of the left wheel joint
right_wheel_name	Name of the right wheel joint

Definition at line 620 of file diff_drive_controller.cpp.

◆ setOdomPubFields()

void diff_drive_controller::DiffDriveController::setOdomPubFields	(	ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)

private

Sets the odometry publishing fields.

Parameters

root_nh	Root node handle
controller_nh	Node handle inside the controller namespace

Definition at line 689 of file diff_drive_controller.cpp.

◆ starting()

void diff_drive_controller::DiffDriveController::starting ( const ros::Time & time )

virtual

Starts controller.

Parameters

time	Current time

Reimplemented from controller_interface::ControllerBase.

Definition at line 506 of file diff_drive_controller.cpp.

◆ stopping()

void diff_drive_controller::DiffDriveController::stopping ( const ros::Time & )

virtual

Stops controller.

Parameters

time	Current time

Reimplemented from controller_interface::ControllerBase.

Definition at line 517 of file diff_drive_controller.cpp.

◆ update()

void diff_drive_controller::DiffDriveController::update	(	const ros::Time &	time,
		const ros::Duration &	period
	)

virtual

Updates controller, i.e. computes the odometry and sets the new velocity commands.

Parameters

time	Current time
period	Time since the last called to update

Implements controller_interface::ControllerBase.

Definition at line 391 of file diff_drive_controller.cpp.

◆ updateDynamicParams()

void diff_drive_controller::DiffDriveController::updateDynamicParams ( )

private

Update the dynamic parameters in the RT loop.

Definition at line 752 of file diff_drive_controller.cpp.

Member Data Documentation

◆ allow_multiple_cmd_vel_publishers_

bool diff_drive_controller::DiffDriveController::allow_multiple_cmd_vel_publishers_

private

Whether to allow multiple publishers on cmd_vel topic or not:

Definition at line 228 of file diff_drive_controller.h.

◆ base_frame_id_

std::string diff_drive_controller::DiffDriveController::base_frame_id_

private

Frame to use for the robot base:

Definition at line 231 of file diff_drive_controller.h.

◆ cmd_vel_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> > diff_drive_controller::DiffDriveController::cmd_vel_pub_

private

Publish executed commands.

Definition at line 203 of file diff_drive_controller.h.

◆ cmd_vel_timeout_

double diff_drive_controller::DiffDriveController::cmd_vel_timeout_

private

Timeout to consider cmd_vel commands old:

Definition at line 225 of file diff_drive_controller.h.

◆ command_

realtime_tools::RealtimeBuffer<Commands> diff_drive_controller::DiffDriveController::command_

private

Definition at line 198 of file diff_drive_controller.h.

◆ command_struct_

Commands diff_drive_controller::DiffDriveController::command_struct_

private

Definition at line 199 of file diff_drive_controller.h.

◆ controller_state_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> > diff_drive_controller::DiffDriveController::controller_state_pub_

private

Controller state publisher.

Definition at line 211 of file diff_drive_controller.h.

◆ dyn_reconf_server_

std::shared_ptr<ReconfigureServer> diff_drive_controller::DiffDriveController::dyn_reconf_server_

private

Definition at line 300 of file diff_drive_controller.h.

◆ dyn_reconf_server_mutex_

boost::recursive_mutex diff_drive_controller::DiffDriveController::dyn_reconf_server_mutex_

private

Definition at line 301 of file diff_drive_controller.h.

◆ dynamic_params_

realtime_tools::RealtimeBuffer<DynamicParams> diff_drive_controller::DiffDriveController::dynamic_params_

private

Definition at line 296 of file diff_drive_controller.h.

◆ enable_odom_tf_

bool diff_drive_controller::DiffDriveController::enable_odom_tf_

private

Whether to publish odometry to tf or not:

Definition at line 237 of file diff_drive_controller.h.

◆ last0_cmd_

Commands diff_drive_controller::DiffDriveController::last0_cmd_

private

Definition at line 244 of file diff_drive_controller.h.

◆ last1_cmd_

Commands diff_drive_controller::DiffDriveController::last1_cmd_

private

Speed limiters:

Definition at line 243 of file diff_drive_controller.h.

◆ last_state_publish_time_

ros::Time diff_drive_controller::DiffDriveController::last_state_publish_time_

private

Definition at line 171 of file diff_drive_controller.h.

◆ left_wheel_joints_

std::vector<hardware_interface::JointHandle> diff_drive_controller::DiffDriveController::left_wheel_joints_

private

Hardware handles:

Definition at line 175 of file diff_drive_controller.h.

◆ left_wheel_radius_multiplier_

double diff_drive_controller::DiffDriveController::left_wheel_radius_multiplier_

private

Definition at line 221 of file diff_drive_controller.h.

◆ limiter_ang_

SpeedLimiter diff_drive_controller::DiffDriveController::limiter_ang_

private

Definition at line 246 of file diff_drive_controller.h.

◆ limiter_lin_

SpeedLimiter diff_drive_controller::DiffDriveController::limiter_lin_

private

Definition at line 245 of file diff_drive_controller.h.

◆ name_

std::string diff_drive_controller::DiffDriveController::name_

private

Definition at line 167 of file diff_drive_controller.h.

◆ odom_frame_id_

std::string diff_drive_controller::DiffDriveController::odom_frame_id_

private

Frame to use for odometry and odom tf:

Definition at line 234 of file diff_drive_controller.h.

◆ odom_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > diff_drive_controller::DiffDriveController::odom_pub_

private

Odometry related:

Definition at line 206 of file diff_drive_controller.h.

◆ odometry_

Odometry diff_drive_controller::DiffDriveController::odometry_

private

Definition at line 208 of file diff_drive_controller.h.

◆ open_loop_

bool diff_drive_controller::DiffDriveController::open_loop_

private

Definition at line 172 of file diff_drive_controller.h.

◆ publish_cmd_

bool diff_drive_controller::DiffDriveController::publish_cmd_

private

Publish limited velocity:

Definition at line 249 of file diff_drive_controller.h.

◆ publish_period_

ros::Duration diff_drive_controller::DiffDriveController::publish_period_

private

Odometry related:

Definition at line 170 of file diff_drive_controller.h.

◆ publish_wheel_joint_controller_state_

bool diff_drive_controller::DiffDriveController::publish_wheel_joint_controller_state_

private

Publish wheel data:

Definition at line 252 of file diff_drive_controller.h.

◆ right_wheel_joints_

std::vector<hardware_interface::JointHandle> diff_drive_controller::DiffDriveController::right_wheel_joints_

private

Definition at line 176 of file diff_drive_controller.h.

◆ right_wheel_radius_multiplier_

double diff_drive_controller::DiffDriveController::right_wheel_radius_multiplier_

private

Definition at line 222 of file diff_drive_controller.h.

◆ sub_command_

ros::Subscriber diff_drive_controller::DiffDriveController::sub_command_

private

Definition at line 200 of file diff_drive_controller.h.

◆ tf_odom_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > diff_drive_controller::DiffDriveController::tf_odom_pub_

private

Definition at line 207 of file diff_drive_controller.h.

◆ time_previous_

ros::Time diff_drive_controller::DiffDriveController::time_previous_

private

Definition at line 179 of file diff_drive_controller.h.

◆ vel_left_desired_previous_

double diff_drive_controller::DiffDriveController::vel_left_desired_previous_

private

Previous velocities from the encoders:

Definition at line 186 of file diff_drive_controller.h.

◆ vel_left_previous_

std::vector<double> diff_drive_controller::DiffDriveController::vel_left_previous_

private

Previous velocities from the encoders:

Definition at line 182 of file diff_drive_controller.h.

◆ vel_right_desired_previous_

double diff_drive_controller::DiffDriveController::vel_right_desired_previous_

private

Definition at line 187 of file diff_drive_controller.h.

◆ vel_right_previous_

std::vector<double> diff_drive_controller::DiffDriveController::vel_right_previous_

private

Definition at line 183 of file diff_drive_controller.h.

◆ wheel_joints_size_

size_t diff_drive_controller::DiffDriveController::wheel_joints_size_

private

Number of wheel joints:

Definition at line 240 of file diff_drive_controller.h.

◆ wheel_radius_

double diff_drive_controller::DiffDriveController::wheel_radius_

private

Wheel radius (assuming it's the same for the left and right wheels):

Definition at line 217 of file diff_drive_controller.h.

◆ wheel_separation_

double diff_drive_controller::DiffDriveController::wheel_separation_

private

Wheel separation, wrt the midpoint of the wheel width:

Definition at line 214 of file diff_drive_controller.h.

◆ wheel_separation_multiplier_

double diff_drive_controller::DiffDriveController::wheel_separation_multiplier_

private

Wheel separation and radius calibration multipliers:

Definition at line 220 of file diff_drive_controller.h.

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

Classes

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ ReconfigureServer

Constructor & Destructor Documentation

◆ DiffDriveController()

Member Function Documentation

◆ brake()

◆ cmdVelCallback()

◆ getWheelNames()

◆ init()

◆ publishWheelData()

◆ reconfCallback()

◆ setOdomParamsFromUrdf()

◆ setOdomPubFields()

◆ starting()

◆ stopping()

◆ update()

◆ updateDynamicParams()

Member Data Documentation

◆ allow_multiple_cmd_vel_publishers_

◆ base_frame_id_

◆ cmd_vel_pub_

◆ cmd_vel_timeout_

◆ command_

◆ command_struct_

◆ controller_state_pub_

◆ dyn_reconf_server_

◆ dyn_reconf_server_mutex_

◆ dynamic_params_

◆ enable_odom_tf_

◆ last0_cmd_

◆ last1_cmd_

◆ last_state_publish_time_

◆ left_wheel_joints_

◆ left_wheel_radius_multiplier_

◆ limiter_ang_

◆ limiter_lin_

◆ name_

◆ odom_frame_id_

◆ odom_pub_

◆ odometry_

◆ open_loop_

◆ publish_cmd_

◆ publish_period_

◆ publish_wheel_joint_controller_state_

◆ right_wheel_joints_

◆ right_wheel_radius_multiplier_

◆ sub_command_

◆ tf_odom_pub_

◆ time_previous_

◆ vel_left_desired_previous_

◆ vel_left_previous_

◆ vel_right_desired_previous_

◆ vel_right_previous_

◆ wheel_joints_size_

◆ wheel_radius_

◆ wheel_separation_

◆ wheel_separation_multiplier_