#include <gimbal_base.h>

Inheritance diagram for rm_gimbal_controllers::Controller:

Public Member Functions
	Controller ()=default

bool	init (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override

void	setDes (const ros::Time &time, double yaw_des, double pitch_des)

void	starting (const ros::Time &time) override

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

Public Member Functions inherited from controller_interface::MultiInterfaceController< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
virtual bool	init (hardware_interface::RobotHW *, ros::NodeHandle &)

	MultiInterfaceController (bool allow_optional_interfaces=false)

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)

virtual void	stopping (const ros::Time &)

virtual void	stopping (const ros::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
enum	{ RATE, TRACK, DIRECT, TRAJ }

Private Member Functions
void	commandCB (const rm_msgs::GimbalCmdConstPtr &msg)

void	direct (const ros::Time &time)

double	feedForward (const ros::Time &time)

std::string	getBaseFrameID (std::unordered_map< int, urdf::JointConstSharedPtr > joint_urdfs)

std::string	getGimbalFrameID (std::unordered_map< int, urdf::JointConstSharedPtr > joint_urdfs)

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

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

void	reconfigCB (rm_gimbal_controllers::GimbalBaseConfig &config, uint32_t)

bool	setDesIntoLimit (const tf2::Quaternion &base2gimbal_des, const urdf::JointConstSharedPtr &joint_urdf, tf2::Quaternion &base2new_des)

void	track (const ros::Time &time)

void	trackCB (const rm_msgs::TrackDataConstPtr &msg)

void	traj (const ros::Time &time)

void	updateChassisVel ()

double	updateCompensation (double chassis_vel_angular_z)

Private Attributes
std::shared_ptr< BulletSolver >	bullet_solver_

double	chassis_compensation_

std::shared_ptr< ChassisVel >	chassis_vel_

rm_msgs::GimbalCmd	cmd_gimbal_

ros::Subscriber	cmd_gimbal_sub_

realtime_tools::RealtimeBuffer< rm_msgs::GimbalCmd >	cmd_rt_buffer_

GimbalConfig	config_ {}

realtime_tools::RealtimeBuffer< GimbalConfig >	config_rt_buffer_

std::unordered_map< int, std::unique_ptr< effort_controllers::JointVelocityController > >	ctrls_

dynamic_reconfigure::Server< rm_gimbal_controllers::GimbalBaseConfig > *	d_srv_ {}

rm_msgs::TrackData	data_track_

ros::Subscriber	data_track_sub_

bool	dynamic_reconfig_initialized_ {}

bool	enable_gravity_compensation_

std::shared_ptr< realtime_tools::RealtimePublisher< rm_msgs::GimbalDesError > >	error_pub_

std::string	gimbal_des_frame_id_ {}

double	gravity_

bool	has_imu_ = true

std::string	imu_name_ {}

hardware_interface::ImuSensorHandle	imu_sensor_handle_

std::unordered_map< int, urdf::JointConstSharedPtr >	joint_urdfs_

geometry_msgs::TransformStamped	last_odom2base_

ros::Time	last_publish_time_ {}

int	loop_count_ {}

geometry_msgs::Vector3	mass_origin_

geometry_msgs::TransformStamped	odom2base_

geometry_msgs::TransformStamped	odom2gimbal_

geometry_msgs::TransformStamped	odom2gimbal_des_

std::unordered_map< int, std::unique_ptr< control_toolbox::Pid > >	pid_pos_

std::unordered_map< int, bool >	pos_des_in_limit_

std::unordered_map< int, std::unique_ptr< realtime_tools::RealtimePublisher< rm_msgs::GimbalPosState > > >	pos_state_pub_

double	publish_rate_ {}

rm_control::RobotStateHandle	robot_state_handle_

bool	start_ = false

int	state_ = RATE

bool	state_changed_ {}

realtime_tools::RealtimeBuffer< rm_msgs::TrackData >	track_rt_buffer_

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::MultiInterfaceController< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
bool	initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources) override

Static Protected Member Functions inherited from controller_interface::MultiInterfaceController< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
static void	clearClaims (hardware_interface::RobotHW *robot_hw)

static void	extractInterfaceResources (hardware_interface::RobotHW robot_hw_in, hardware_interface::RobotHW robot_hw_out)

static bool	hasRequiredInterfaces (hardware_interface::RobotHW *robot_hw)

static void	populateClaimedResources (hardware_interface::RobotHW *robot_hw, ClaimedResources &claimed_resources)

Protected Attributes inherited from controller_interface::MultiInterfaceController< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >
bool	allow_optional_interfaces_

hardware_interface::RobotHW	robot_hw_ctrl_

Detailed Description

Definition at line 162 of file gimbal_base.h.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

private

Enumerator
RATE
TRACK
DIRECT
TRAJ

Definition at line 233 of file gimbal_base.h.

Constructor & Destructor Documentation

◆ Controller()

rm_gimbal_controllers::Controller::Controller ( )

default

Member Function Documentation

◆ commandCB()

void Controller::commandCB ( const rm_msgs::GimbalCmdConstPtr & msg )

private

Definition at line 617 of file gimbal_base.cpp.

◆ direct()

void Controller::direct ( const ros::Time & time )

private

Definition at line 340 of file gimbal_base.cpp.

◆ feedForward()

double Controller::feedForward ( const ros::Time & time )

private

Definition at line 545 of file gimbal_base.cpp.

◆ getBaseFrameID()

std::string Controller::getBaseFrameID ( std::unordered_map< int, urdf::JointConstSharedPtr > joint_urdfs )

private

Definition at line 598 of file gimbal_base.cpp.

◆ getGimbalFrameID()

std::string Controller::getGimbalFrameID ( std::unordered_map< int, urdf::JointConstSharedPtr > joint_urdfs )

private

Definition at line 587 of file gimbal_base.cpp.

◆ init()

bool Controller::init	(	hardware_interface::RobotHW *	robot_hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)

overridevirtual

Reimplemented from controller_interface::MultiInterfaceController< rm_control::RobotStateInterface, hardware_interface::ImuSensorInterface, hardware_interface::EffortJointInterface >.

Definition at line 80 of file gimbal_base.cpp.

◆ moveJoint()

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

private

Definition at line 425 of file gimbal_base.cpp.

◆ rate()

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

private

Definition at line 248 of file gimbal_base.cpp.

◆ reconfigCB()

void Controller::reconfigCB	(	rm_gimbal_controllers::GimbalBaseConfig &	config,
		uint32_t
	)

private

Definition at line 629 of file gimbal_base.cpp.

◆ setDes()

void Controller::setDes	(	const ros::Time &	time,
		double	yaw_des,
		double	pitch_des
	)

Definition at line 235 of file gimbal_base.cpp.

◆ setDesIntoLimit()

bool Controller::setDesIntoLimit	(	const tf2::Quaternion &	base2gimbal_des,
		const urdf::JointConstSharedPtr &	joint_urdf,
		tf2::Quaternion &	base2new_des
	)

private

Definition at line 398 of file gimbal_base.cpp.

◆ starting()

void Controller::starting ( const ros::Time & time )

overridevirtual

Reimplemented from controller_interface::ControllerBase.

Definition at line 189 of file gimbal_base.cpp.

◆ track()

void Controller::track ( const ros::Time & time )

private

Definition at line 270 of file gimbal_base.cpp.

◆ trackCB()

void Controller::trackCB ( const rm_msgs::TrackDataConstPtr & msg )

private

Definition at line 622 of file gimbal_base.cpp.

◆ traj()

void Controller::traj ( const ros::Time & time )

private

Definition at line 367 of file gimbal_base.cpp.

◆ update()

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

overridevirtual

Implements controller_interface::ControllerBase.

Definition at line 196 of file gimbal_base.cpp.

◆ updateChassisVel()

void Controller::updateChassisVel ( )

private

Definition at line 567 of file gimbal_base.cpp.

◆ updateCompensation()

double Controller::updateCompensation ( double chassis_vel_angular_z )

private

Definition at line 609 of file gimbal_base.cpp.

Member Data Documentation

◆ bullet_solver_

std::shared_ptr<BulletSolver> rm_gimbal_controllers::Controller::bullet_solver_

private

Definition at line 198 of file gimbal_base.h.

◆ chassis_compensation_

double rm_gimbal_controllers::Controller::chassis_compensation_

private

Definition at line 226 of file gimbal_base.h.

◆ chassis_vel_

std::shared_ptr<ChassisVel> rm_gimbal_controllers::Controller::chassis_vel_

private

Definition at line 225 of file gimbal_base.h.

◆ cmd_gimbal_

rm_msgs::GimbalCmd rm_gimbal_controllers::Controller::cmd_gimbal_

private

Definition at line 209 of file gimbal_base.h.

◆ cmd_gimbal_sub_

ros::Subscriber rm_gimbal_controllers::Controller::cmd_gimbal_sub_

private

Definition at line 204 of file gimbal_base.h.

◆ cmd_rt_buffer_

realtime_tools::RealtimeBuffer<rm_msgs::GimbalCmd> rm_gimbal_controllers::Controller::cmd_rt_buffer_

private

Definition at line 206 of file gimbal_base.h.

◆ config_

GimbalConfig rm_gimbal_controllers::Controller::config_ {}

private

Definition at line 229 of file gimbal_base.h.

◆ config_rt_buffer_

realtime_tools::RealtimeBuffer<GimbalConfig> rm_gimbal_controllers::Controller::config_rt_buffer_

private

Definition at line 230 of file gimbal_base.h.

◆ ctrls_

std::unordered_map<int, std::unique_ptr<effort_controllers::JointVelocityController> > rm_gimbal_controllers::Controller::ctrls_

private

Definition at line 192 of file gimbal_base.h.

◆ d_srv_

dynamic_reconfigure::Server<rm_gimbal_controllers::GimbalBaseConfig>* rm_gimbal_controllers::Controller::d_srv_ {}

private

Definition at line 231 of file gimbal_base.h.

◆ data_track_

rm_msgs::TrackData rm_gimbal_controllers::Controller::data_track_

private

Definition at line 210 of file gimbal_base.h.

◆ data_track_sub_

ros::Subscriber rm_gimbal_controllers::Controller::data_track_sub_

private

Definition at line 205 of file gimbal_base.h.

◆ dynamic_reconfig_initialized_

bool rm_gimbal_controllers::Controller::dynamic_reconfig_initialized_ {}

private

Definition at line 228 of file gimbal_base.h.

◆ enable_gravity_compensation_

bool rm_gimbal_controllers::Controller::enable_gravity_compensation_

private

Definition at line 222 of file gimbal_base.h.

◆ error_pub_

std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalDesError> > rm_gimbal_controllers::Controller::error_pub_

private

Definition at line 203 of file gimbal_base.h.

◆ gimbal_des_frame_id_

std::string rm_gimbal_controllers::Controller::gimbal_des_frame_id_ {}

private

Definition at line 211 of file gimbal_base.h.

◆ gravity_

double rm_gimbal_controllers::Controller::gravity_

private

Definition at line 221 of file gimbal_base.h.

◆ has_imu_

bool rm_gimbal_controllers::Controller::has_imu_ = true

private

Definition at line 196 of file gimbal_base.h.

◆ imu_name_

std::string rm_gimbal_controllers::Controller::imu_name_ {}

private

Definition at line 211 of file gimbal_base.h.

◆ imu_sensor_handle_

hardware_interface::ImuSensorHandle rm_gimbal_controllers::Controller::imu_sensor_handle_

private

Definition at line 191 of file gimbal_base.h.

◆ joint_urdfs_

std::unordered_map<int, urdf::JointConstSharedPtr> rm_gimbal_controllers::Controller::joint_urdfs_

private

Definition at line 194 of file gimbal_base.h.

◆ last_odom2base_

geometry_msgs::TransformStamped rm_gimbal_controllers::Controller::last_odom2base_

private

Definition at line 217 of file gimbal_base.h.

◆ last_publish_time_

ros::Time rm_gimbal_controllers::Controller::last_publish_time_ {}

private

Definition at line 201 of file gimbal_base.h.

◆ loop_count_

int rm_gimbal_controllers::Controller::loop_count_ {}

private

Definition at line 214 of file gimbal_base.h.

◆ mass_origin_

geometry_msgs::Vector3 rm_gimbal_controllers::Controller::mass_origin_

private

Definition at line 220 of file gimbal_base.h.

◆ odom2base_

geometry_msgs::TransformStamped rm_gimbal_controllers::Controller::odom2base_

private

Definition at line 217 of file gimbal_base.h.

◆ odom2gimbal_

geometry_msgs::TransformStamped rm_gimbal_controllers::Controller::odom2gimbal_

private

Definition at line 217 of file gimbal_base.h.

◆ odom2gimbal_des_

geometry_msgs::TransformStamped rm_gimbal_controllers::Controller::odom2gimbal_des_

private

Definition at line 217 of file gimbal_base.h.

◆ pid_pos_

std::unordered_map<int, std::unique_ptr<control_toolbox::Pid> > rm_gimbal_controllers::Controller::pid_pos_

private

Definition at line 193 of file gimbal_base.h.

◆ pos_des_in_limit_

std::unordered_map<int, bool> rm_gimbal_controllers::Controller::pos_des_in_limit_

private

Definition at line 195 of file gimbal_base.h.

◆ pos_state_pub_

std::unordered_map<int, std::unique_ptr<realtime_tools::RealtimePublisher<rm_msgs::GimbalPosState> > > rm_gimbal_controllers::Controller::pos_state_pub_

private

Definition at line 202 of file gimbal_base.h.

◆ publish_rate_

double rm_gimbal_controllers::Controller::publish_rate_ {}

private

Definition at line 212 of file gimbal_base.h.

◆ robot_state_handle_

rm_control::RobotStateHandle rm_gimbal_controllers::Controller::robot_state_handle_

private

Definition at line 190 of file gimbal_base.h.

◆ start_

bool rm_gimbal_controllers::Controller::start_ = false

private

Definition at line 241 of file gimbal_base.h.

◆ state_

int rm_gimbal_controllers::Controller::state_ = RATE

private

Definition at line 240 of file gimbal_base.h.

◆ state_changed_

bool rm_gimbal_controllers::Controller::state_changed_ {}

private

Definition at line 213 of file gimbal_base.h.

◆ track_rt_buffer_

realtime_tools::RealtimeBuffer<rm_msgs::TrackData> rm_gimbal_controllers::Controller::track_rt_buffer_

private

Definition at line 207 of file gimbal_base.h.

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

Public Member Functions

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Enumeration Documentation

◆ anonymous enum

Constructor & Destructor Documentation

◆ Controller()

Member Function Documentation

◆ commandCB()

◆ direct()

◆ feedForward()

◆ getBaseFrameID()

◆ getGimbalFrameID()

◆ init()

◆ moveJoint()

◆ rate()

◆ reconfigCB()

◆ setDes()

◆ setDesIntoLimit()

◆ starting()

◆ track()

◆ trackCB()

◆ traj()

◆ update()

◆ updateChassisVel()

◆ updateCompensation()

Member Data Documentation

◆ bullet_solver_

◆ chassis_compensation_

◆ chassis_vel_

◆ cmd_gimbal_

◆ cmd_gimbal_sub_

◆ cmd_rt_buffer_

◆ config_

◆ config_rt_buffer_

◆ ctrls_

◆ d_srv_

◆ data_track_

◆ data_track_sub_

◆ dynamic_reconfig_initialized_

◆ enable_gravity_compensation_

◆ error_pub_

◆ gimbal_des_frame_id_

◆ gravity_

◆ has_imu_

◆ imu_name_

◆ imu_sensor_handle_

◆ joint_urdfs_

◆ last_odom2base_

◆ last_publish_time_

◆ loop_count_

◆ mass_origin_

◆ odom2base_

◆ odom2gimbal_

◆ odom2gimbal_des_

◆ pid_pos_

◆ pos_des_in_limit_

◆ pos_state_pub_

◆ publish_rate_

◆ robot_state_handle_

◆ start_

◆ state_

◆ state_changed_

◆ track_rt_buffer_