#include <chassis_base.h>

Inheritance diagram for rm_chassis_controllers::ChassisBase< T >:

Collaboration diagram for rm_chassis_controllers::ChassisBase< T >:

[legend]

Public Member Functions
	ChassisBase ()=default

bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
	Get and check params for covariances. Setup odometry realtime publisher + odom message constant fields. init odom tf.

void	update (const ros::Time &time, const ros::Duration &period) override
	Receive real_time command from manual. Execute different action according to current mode. Set necessary params of chassis. Execute power limit.

Protected Types
enum	{ RAW , FOLLOW , TWIST }

Protected Member Functions
void	raw ()

virtual void	follow (const ros::Time &time, const ros::Duration &period)
	The mode FOLLOW: chassis will follow gimbal.

void	twist (const ros::Time &time, const ros::Duration &period)
	The mode TWIST: Just moving chassis.

virtual void	moveJoint (const ros::Time &time, const ros::Duration &period)=0

virtual geometry_msgs::Twist	odometry ()=0

void	updateOdom (const ros::Time &time, const ros::Duration &period)
	Init frame on base_link. Integral vel to current_pos_ and angle.

void	recovery ()
	Set chassis velocity to zero.

void	tfVelToBase (const std::string &from)
	Transform tf velocity to base link frame.

virtual void	powerLimit ()
	To limit the chassis power according to current power limit.

virtual void	updatePowerStatus ()

void	cmdChassisCallback (const rm_msgs::ChassisCmdConstPtr &msg)
	Write current command from rm_msgs::ChassisCmd.

void	cmdVelCallback (const geometry_msgs::Twist::ConstPtr &msg)
	Write current command from geometry_msgs::Twist.

void	initialize_parameters (ros::NodeHandle &controller_nh)

void	slamCallback (const nav_msgs::Odometry::ConstPtr &msg)

void	localizationCallback (const geometry_msgs::TransformStamped::ConstPtr &msg)

void	capacityCallback (const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr &msg)

Protected Attributes
rm_control::RobotStateHandle	robot_state_handle_ {}

hardware_interface::EffortJointInterface *	effort_joint_interface_ {}

std::vector< hardware_interface::JointHandle >	joint_handles_ {}

realtime_tools::RealtimeBuffer< Command >	cmd_rt_buffer_ {}

realtime_tools::RealtimeBuffer< nav_msgs::Odometry >	slam_rt_buffer_ {}

realtime_tools::RealtimeBuffer< geometry_msgs::TransformStamped >	localization_rt_buffer_ {}

std::unique_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > >	odom_pub_

std::unique_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64 > >	cpower_pub_

std::unique_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64 > >	epower_pub_

std::unique_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64 > >	chassis_power_pub_

rm_common::TfRtBroadcaster	brcst4global_map2robot_odom_ {}

rm_common::TfRtBroadcaster	brcst4global_map2camera_init_ {}

rm_common::TfRtBroadcaster	brcst4robot_odom2robot_base_ {}

geometry_msgs::TransformStamped	global_map2robot_odom_ {}

geometry_msgs::TransformStamped	global_map2camera_init_ {}

geometry_msgs::TransformStamped	robot_odom2robot_base_ {}

geometry_msgs::TransformStamped	robot_base2lidar_base_ {}

tf2::Transform	T_global_map2robot_odom_ {}

tf2::Transform	T_robot_odom_2robot_base_ {}

tf2::Transform	T_lidar_odom2lidar_base_ {}

tf2::Transform	T_robot_base2lidar_base_ {}

tf2::Transform	T_global_map2lidar_odom_ {}

ros::Subscriber	cmd_vel_sub_

ros::Subscriber	cmd_chassis_sub_

ros::Subscriber	slam_sub_

ros::Subscriber	localization_sub_

ros::Subscriber	capacity_sub_

std::unique_ptr< RampFilter< double > >	ramp_x_ { nullptr }

std::unique_ptr< RampFilter< double > >	ramp_y_ { nullptr }

std::unique_ptr< RampFilter< double > >	ramp_w_ { nullptr }

double	publish_rate_ { 100.0 }

bool	publish_map_tf_ { false }

bool	publish_odom_tf_ { false }

bool	gravity_estimation_offset_ { false }

double	roll_ { 0.0 }

double	pitch_ { 0.0 }

double	yaw_ { 0.0 }

double	wheel_radius_ { 0.02 }

double	twist_angular_ { M_PI / 6 }

double	max_odom_vel_ { 10.0 }

double	timeout_ { 0.1 }

double	chassis_power_ { 0.0 }

bool	capacity_update_flag_ { false }

bool	use_rls_ { false }

bool	use_K_angle_ { false }

bool	odom_initialized_ { false }

bool	slam_updated_ { false }

bool	localization_updated_ { false }

bool	state_changed_ { true }

int	state_ { RAW }

std::string	follow_source_frame_ {}

std::string	command_source_frame_ {}

std::string	global_map_frame_id_ { "map" }

std::string	robot_odom_frame_id_ { "odom" }

std::string	robot_base_frame_id_ { "base_link" }

std::string	lidar_base_frame_id_ { "livox_frame" }

std::string	slam_topic_ { "/slam/odometry" }

std::string	localization_topic_ { "/shinji/result" }

std::string	capacity_topic_ { "/rm_referee/power_management/sample_and_status" }

ros::Time	last_publish_time_ {}

geometry_msgs::Vector3	vel_cmd_ {}

std::array< double, 36 >	twist_covariance_ {}

control_toolbox::Pid	pid_follow_ {}

Command	cmd_struct_ {}

PowerLimitor	wheel_power_limitor_ {}

Member Enumeration Documentation

◆ anonymous enum

template<typename... T>

anonymous enum

protected

Enumerator
RAW
FOLLOW
TWIST

Constructor & Destructor Documentation

◆ ChassisBase()

template<typename... T>

rm_chassis_controllers::ChassisBase< T >::ChassisBase ( )

default

Member Function Documentation

◆ capacityCallback()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::capacityCallback ( const rm_msgs::PowerManagementSampleAndStatusData::ConstPtr & msg )

protected

◆ cmdChassisCallback()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::cmdChassisCallback ( const rm_msgs::ChassisCmdConstPtr & msg )

protected

Write current command from rm_msgs::ChassisCmd.

Parameters

msg	This message contains various state parameter settings for basic chassis control

◆ cmdVelCallback()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr & msg )

protected

Write current command from geometry_msgs::Twist.

Parameters

msg	This expresses velocity in free space broken into its linear and angular parts.

◆ follow()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::follow	(	const ros::Time &	time,
		const ros::Duration &	period )

protectedvirtual

The mode FOLLOW: chassis will follow gimbal.

The mode FOLLOW: The chassis's direct will follow gimbal's direct all the time.

Parameters

time	The current time.
period	The time passed since the last call to update.

◆ init()

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::init	(	hardware_interface::RobotHW *	robot_hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh )

override

Get and check params for covariances. Setup odometry realtime publisher + odom message constant fields. init odom tf.

Parameters

robot_hw	The robot hardware abstraction.
root_nh	A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).
controller_nh	A NodeHandle in the namespace of the controller. This is where the controller-specific configuration resides.

Returns: True if initialization was successful and the controller is ready to be started.

◆ initialize_parameters()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::initialize_parameters ( ros::NodeHandle & controller_nh )

protected

◆ localizationCallback()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::localizationCallback ( const geometry_msgs::TransformStamped::ConstPtr & msg )

protected

◆ moveJoint()

template<typename... T>

virtual void rm_chassis_controllers::ChassisBase< T >::moveJoint	(	const ros::Time &	time,
		const ros::Duration &	period )

protectedpure virtual

Implemented in rm_chassis_controllers::BipedalController, and rm_chassis_controllers::OmniController.

◆ odometry()

template<typename... T>

virtual geometry_msgs::Twist rm_chassis_controllers::ChassisBase< T >::odometry ( )

protectedpure virtual

◆ powerLimit()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::powerLimit ( )

protectedvirtual

To limit the chassis power according to current power limit.

Receive power limit from command. Set max_effort command to chassis to avoid exceed power limit.

◆ raw()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::raw ( )

protected

@briefThe The mode RAW: original state.

The mode raw: original state. Linear velocity will be set zero to stop move.

◆ recovery()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::recovery ( )

protected

Set chassis velocity to zero.

◆ slamCallback()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::slamCallback ( const nav_msgs::Odometry::ConstPtr & msg )

protected

◆ tfVelToBase()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::tfVelToBase ( const std::string & from )

protected

Transform tf velocity to base link frame.

Parameters

from	The father frame.

◆ twist()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::twist	(	const ros::Time &	time,
		const ros::Duration &	period )

protected

The mode TWIST: Just moving chassis.

The mode TWIST: Chassis will move independent and will not effect by gimbal's move.

Parameters

time	The current time.
period	The time passed since the last call to update.

◆ update()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::update	(	const ros::Time &	time,
		const ros::Duration &	period )

override

Receive real_time command from manual. Execute different action according to current mode. Set necessary params of chassis. Execute power limit.

Receive real_time command from manual and check whether it is normally, if can not receive command from manual for a while, chassis's velocity will be set zero to avoid out of control. Execute different action according to current mode such as RAW, FOLLOW, TWIST.(Their specific usage will be explain in the next). UpdateOdom,Set necessary params such as Acc and vel_tfed. Execute moving action and powerlimit.

Parameters

time	The current time.
period	The time passed since the last call to update.

◆ updateOdom()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::updateOdom	(	const ros::Time &	time,
		const ros::Duration &	period )

protected

Init frame on base_link. Integral vel to current_pos_ and angle.

Parameters

time	The current time.
period	The time passed since the last call to update.

◆ updatePowerStatus()

template<typename... T>

void rm_chassis_controllers::ChassisBase< T >::updatePowerStatus ( )

protectedvirtual

Member Data Documentation

◆ brcst4global_map2camera_init_

template<typename... T>

rm_common::TfRtBroadcaster rm_chassis_controllers::ChassisBase< T >::brcst4global_map2camera_init_ {}

protected

◆ brcst4global_map2robot_odom_

template<typename... T>

rm_common::TfRtBroadcaster rm_chassis_controllers::ChassisBase< T >::brcst4global_map2robot_odom_ {}

protected

◆ brcst4robot_odom2robot_base_

template<typename... T>

rm_common::TfRtBroadcaster rm_chassis_controllers::ChassisBase< T >::brcst4robot_odom2robot_base_ {}

protected

◆ capacity_sub_

template<typename... T>

ros::Subscriber rm_chassis_controllers::ChassisBase< T >::capacity_sub_

protected

◆ capacity_topic_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::capacity_topic_ { "/rm_referee/power_management/sample_and_status" }

protected

◆ capacity_update_flag_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::capacity_update_flag_ { false }

protected

◆ chassis_power_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::chassis_power_ { 0.0 }

protected

◆ chassis_power_pub_

template<typename... T>

std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > rm_chassis_controllers::ChassisBase< T >::chassis_power_pub_

protected

◆ cmd_chassis_sub_

template<typename... T>

ros::Subscriber rm_chassis_controllers::ChassisBase< T >::cmd_chassis_sub_

protected

◆ cmd_rt_buffer_

template<typename... T>

realtime_tools::RealtimeBuffer<Command> rm_chassis_controllers::ChassisBase< T >::cmd_rt_buffer_ {}

protected

◆ cmd_struct_

template<typename... T>

Command rm_chassis_controllers::ChassisBase< T >::cmd_struct_ {}

protected

◆ cmd_vel_sub_

template<typename... T>

ros::Subscriber rm_chassis_controllers::ChassisBase< T >::cmd_vel_sub_

protected

◆ command_source_frame_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::command_source_frame_ {}

protected

◆ cpower_pub_

template<typename... T>

std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > rm_chassis_controllers::ChassisBase< T >::cpower_pub_

protected

◆ effort_joint_interface_

template<typename... T>

hardware_interface::EffortJointInterface* rm_chassis_controllers::ChassisBase< T >::effort_joint_interface_ {}

protected

◆ epower_pub_

template<typename... T>

std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > rm_chassis_controllers::ChassisBase< T >::epower_pub_

protected

◆ follow_source_frame_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::follow_source_frame_ {}

protected

◆ global_map2camera_init_

template<typename... T>

geometry_msgs::TransformStamped rm_chassis_controllers::ChassisBase< T >::global_map2camera_init_ {}

protected

◆ global_map2robot_odom_

template<typename... T>

geometry_msgs::TransformStamped rm_chassis_controllers::ChassisBase< T >::global_map2robot_odom_ {}

protected

◆ global_map_frame_id_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::global_map_frame_id_ { "map" }

protected

◆ gravity_estimation_offset_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::gravity_estimation_offset_ { false }

protected

◆ joint_handles_

template<typename... T>

std::vector<hardware_interface::JointHandle> rm_chassis_controllers::ChassisBase< T >::joint_handles_ {}

protected

◆ last_publish_time_

template<typename... T>

ros::Time rm_chassis_controllers::ChassisBase< T >::last_publish_time_ {}

protected

◆ lidar_base_frame_id_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::lidar_base_frame_id_ { "livox_frame" }

protected

◆ localization_rt_buffer_

template<typename... T>

realtime_tools::RealtimeBuffer<geometry_msgs::TransformStamped> rm_chassis_controllers::ChassisBase< T >::localization_rt_buffer_ {}

protected

◆ localization_sub_

template<typename... T>

ros::Subscriber rm_chassis_controllers::ChassisBase< T >::localization_sub_

protected

◆ localization_topic_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::localization_topic_ { "/shinji/result" }

protected

◆ localization_updated_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::localization_updated_ { false }

protected

◆ max_odom_vel_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::max_odom_vel_ { 10.0 }

protected

◆ odom_initialized_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::odom_initialized_ { false }

protected

◆ odom_pub_

template<typename... T>

std::unique_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > rm_chassis_controllers::ChassisBase< T >::odom_pub_

protected

◆ pid_follow_

template<typename... T>

control_toolbox::Pid rm_chassis_controllers::ChassisBase< T >::pid_follow_ {}

protected

◆ pitch_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::pitch_ { 0.0 }

protected

◆ publish_map_tf_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::publish_map_tf_ { false }

protected

◆ publish_odom_tf_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::publish_odom_tf_ { false }

protected

◆ publish_rate_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::publish_rate_ { 100.0 }

protected

◆ ramp_w_

template<typename... T>

std::unique_ptr<RampFilter<double> > rm_chassis_controllers::ChassisBase< T >::ramp_w_ { nullptr }

protected

◆ ramp_x_

template<typename... T>

std::unique_ptr<RampFilter<double> > rm_chassis_controllers::ChassisBase< T >::ramp_x_ { nullptr }

protected

◆ ramp_y_

template<typename... T>

std::unique_ptr<RampFilter<double> > rm_chassis_controllers::ChassisBase< T >::ramp_y_ { nullptr }

protected

◆ robot_base2lidar_base_

template<typename... T>

geometry_msgs::TransformStamped rm_chassis_controllers::ChassisBase< T >::robot_base2lidar_base_ {}

protected

◆ robot_base_frame_id_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::robot_base_frame_id_ { "base_link" }

protected

◆ robot_odom2robot_base_

template<typename... T>

geometry_msgs::TransformStamped rm_chassis_controllers::ChassisBase< T >::robot_odom2robot_base_ {}

protected

◆ robot_odom_frame_id_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::robot_odom_frame_id_ { "odom" }

protected

◆ robot_state_handle_

template<typename... T>

rm_control::RobotStateHandle rm_chassis_controllers::ChassisBase< T >::robot_state_handle_ {}

protected

◆ roll_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::roll_ { 0.0 }

protected

◆ slam_rt_buffer_

template<typename... T>

realtime_tools::RealtimeBuffer<nav_msgs::Odometry> rm_chassis_controllers::ChassisBase< T >::slam_rt_buffer_ {}

protected

◆ slam_sub_

template<typename... T>

ros::Subscriber rm_chassis_controllers::ChassisBase< T >::slam_sub_

protected

◆ slam_topic_

template<typename... T>

std::string rm_chassis_controllers::ChassisBase< T >::slam_topic_ { "/slam/odometry" }

protected

◆ slam_updated_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::slam_updated_ { false }

protected

◆ state_

template<typename... T>

int rm_chassis_controllers::ChassisBase< T >::state_ { RAW }

protected

◆ state_changed_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::state_changed_ { true }

protected

◆ T_global_map2lidar_odom_

template<typename... T>

tf2::Transform rm_chassis_controllers::ChassisBase< T >::T_global_map2lidar_odom_ {}

protected

◆ T_global_map2robot_odom_

template<typename... T>

tf2::Transform rm_chassis_controllers::ChassisBase< T >::T_global_map2robot_odom_ {}

protected

◆ T_lidar_odom2lidar_base_

template<typename... T>

tf2::Transform rm_chassis_controllers::ChassisBase< T >::T_lidar_odom2lidar_base_ {}

protected

◆ T_robot_base2lidar_base_

template<typename... T>

tf2::Transform rm_chassis_controllers::ChassisBase< T >::T_robot_base2lidar_base_ {}

protected

◆ T_robot_odom_2robot_base_

template<typename... T>

tf2::Transform rm_chassis_controllers::ChassisBase< T >::T_robot_odom_2robot_base_ {}

protected

◆ timeout_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::timeout_ { 0.1 }

protected

◆ twist_angular_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::twist_angular_ { M_PI / 6 }

protected

◆ twist_covariance_

template<typename... T>

std::array<double, 36> rm_chassis_controllers::ChassisBase< T >::twist_covariance_ {}

protected

◆ use_K_angle_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::use_K_angle_ { false }

protected

◆ use_rls_

template<typename... T>

bool rm_chassis_controllers::ChassisBase< T >::use_rls_ { false }

protected

◆ vel_cmd_

template<typename... T>

geometry_msgs::Vector3 rm_chassis_controllers::ChassisBase< T >::vel_cmd_ {}

protected

◆ wheel_power_limitor_

template<typename... T>

PowerLimitor rm_chassis_controllers::ChassisBase< T >::wheel_power_limitor_ {}

protected

◆ wheel_radius_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::wheel_radius_ { 0.02 }

protected

◆ yaw_

template<typename... T>

double rm_chassis_controllers::ChassisBase< T >::yaw_ { 0.0 }

protected

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

rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h
rm_chassis_controllers/src/chassis_base.cpp

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ ChassisBase()

Member Function Documentation

◆ capacityCallback()

◆ cmdChassisCallback()

◆ cmdVelCallback()

◆ follow()

◆ init()

◆ initialize_parameters()

◆ localizationCallback()

◆ moveJoint()

◆ odometry()

◆ powerLimit()

◆ raw()

◆ recovery()

◆ slamCallback()

◆ tfVelToBase()

◆ twist()

◆ update()

◆ updateOdom()

◆ updatePowerStatus()

Member Data Documentation

◆ brcst4global_map2camera_init_

◆ brcst4global_map2robot_odom_

◆ brcst4robot_odom2robot_base_

◆ capacity_sub_

◆ capacity_topic_

◆ capacity_update_flag_

◆ chassis_power_

◆ chassis_power_pub_

◆ cmd_chassis_sub_

◆ cmd_rt_buffer_

◆ cmd_struct_

◆ cmd_vel_sub_

◆ command_source_frame_

◆ cpower_pub_

◆ effort_joint_interface_

◆ epower_pub_

◆ follow_source_frame_

◆ global_map2camera_init_

◆ global_map2robot_odom_

◆ global_map_frame_id_

◆ gravity_estimation_offset_

◆ joint_handles_

◆ last_publish_time_

◆ lidar_base_frame_id_

◆ localization_rt_buffer_

◆ localization_sub_

◆ localization_topic_

◆ localization_updated_

◆ max_odom_vel_

◆ odom_initialized_

◆ odom_pub_

◆ pid_follow_

◆ pitch_

◆ publish_map_tf_

◆ publish_odom_tf_

◆ publish_rate_

◆ ramp_w_

◆ ramp_x_

◆ ramp_y_

◆ robot_base2lidar_base_

◆ robot_base_frame_id_

◆ robot_odom2robot_base_

◆ robot_odom_frame_id_

◆ robot_state_handle_

◆ roll_

◆ slam_rt_buffer_

◆ slam_sub_

◆ slam_topic_

◆ slam_updated_

◆ state_

◆ state_changed_

◆ T_global_map2lidar_odom_