#include <robot_driver.h>

Public Member Functions
	RobotDriver (ros::NodeHandle nh, int argc, char **argv)
	Constructor for RobotDriver.

void	spin ()
	Calls ros spinOnce and pubs data at set frequency.

Private Member Functions
void	initLegController ()
	Initializes leg controller object.

void	initStateControlStructs ()
	Initializes states and controls structures.

void	initStateEstimator ()
	Initializes state estimator object.

void	controlModeCallback (const std_msgs::UInt8::ConstPtr &msg)
	Verifies and updates new control mode.

void	localPlanCallback (const quad_msgs::RobotPlan::ConstPtr &msg)
	Callback function to handle new local plan (states and GRFs)

void	robotStateCallback (const quad_msgs::RobotState::ConstPtr &msg)
	Callback function to handle current robot state.

void	mocapCallback (const geometry_msgs::PoseStamped::ConstPtr &msg)
	Callback function to handle current robot pose.

void	trajectoryStateCallback (const quad_msgs::RobotState::ConstPtr &msg)
	Callback function to handle reference trajectory state.

void	singleJointCommandCallback (const geometry_msgs::Vector3::ConstPtr &msg)
	Callback to handle new leg override commands.

void	controlRestartFlagCallback (const std_msgs::Bool::ConstPtr &msg)
	Callback to handle control restart flag messages.

void	remoteHeartbeatCallback (const std_msgs::Header::ConstPtr &msg)
	Callback to handle new remote heartbeat messages.

void	checkMessagesForSafety ()
	Check to make sure required messages are fresh.

bool	updateState ()
	Update the most recent state message with the given data.

bool	updateControl ()
	Function to compute leg command array message.

void	publishState ()
	Publish the most recent state message with the given data.

void	publishControl (bool is_valid)
	Function to publish leg command array message.

void	publishHeartbeat ()
	Function to publish heartbeat message.

Private Attributes
ros::Subscriber	control_mode_sub_
	Subscriber for control mode.

ros::Subscriber	body_plan_sub_
	ROS subscriber for body plan.

ros::Subscriber	local_plan_sub_
	ROS subscriber for local plan.

ros::Subscriber	mocap_sub_
	ROS subscriber for local plan.

ros::Subscriber	robot_state_sub_
	ROS subscriber for state estimate.

ros::Subscriber	control_restart_flag_sub_
	ROS subscriber for control restart flag.

ros::Publisher	robot_state_pub_
	ROS publisher for ground truth state.

ros::Publisher	trajectry_robot_state_pub_

ros::Subscriber	remote_heartbeat_sub_
	ROS subscriber for remote heartbeat.

ros::Subscriber	single_joint_cmd_sub_
	ROS subscriber for single joint command.

ros::Publisher	robot_heartbeat_pub_
	ROS publisher for robot heartbeat.

ros::Publisher	leg_command_array_pub_
	ROS publisher for inverse dynamics.

ros::Publisher	grf_pub_
	ROS publisher for desired GRF.

ros::Publisher	imu_pub_
	ROS publisher for imu data.

ros::Publisher	joint_state_pub_
	ROS publisher for joint data.

ros::NodeHandle	nh_
	Nodehandle to pub to and sub from.

bool	is_hardware_
	Boolean for whether robot layer is hardware (else sim)

std::string	controller_id_
	Controller type.

std::string	estimator_id_
	Estimator type.

double	update_rate_
	Update rate for computing new controls;.

double	publish_rate_
	Update rate for publishing data to ROS;.

const int	num_feet_ = 4
	Number of feet.

int	control_mode_
	Robot mode.

std::vector< double >	torque_limits_
	Torque limits.

const int	SIT = 0
	Define ids for control modes: Sit.

const int	READY = 1
	Define ids for control modes: Stand.

const int	SIT_TO_READY = 2
	Define ids for control modes: Sit to stand.

const int	READY_TO_SIT = 3
	Define ids for control modes: Stand to sit.

const int	SAFETY = 4
	Define ids for control modes: Safety.

const int	NONE = 0
	Define ids for input types: none.

const int	LOCAL_PLAN = 1
	Define ids for input types: local plan.

const int	GRFS = 2
	Define ids for input types: grf array.

quad_msgs::RobotPlan::ConstPtr	last_local_plan_msg_
	Most recent local plan.

quad_msgs::RobotState	last_robot_state_msg_
	Most recent state estimate.

quad_msgs::GRFArray::ConstPtr	last_grf_array_msg_
	Most recent local plan.

std_msgs::Header::ConstPtr	last_remote_heartbeat_msg_
	Most recent remote heartbeat.

std_msgs::Header	last_robot_heartbeat_msg_
	Most recent robot heartbeat.

double	last_state_time_

double	remote_heartbeat_received_time_

const double	transition_duration_ = 1.0
	Duration for sit to stand behavior.

double	input_timeout_
	Timeout (in s) for receiving new input reference messages.

double	state_timeout_
	Timeout (in s) for receiving new state messages.

double	heartbeat_timeout_
	Timeout (in s) for receiving new heartbeat messages.

double	remote_latency_threshold_warn_
	Latency threshold on robot messages for warnings (s)

double	remote_latency_threshold_error_
	Latency threshold on robot messages for error (s)

quad_msgs::LegCommandArray	leg_command_array_msg_
	Message for leg command array.

quad_msgs::GRFArray	grf_array_msg_
	Message for leg command array.

Eigen::VectorXd	user_tx_data_
	User transmission data.

Eigen::VectorXd	user_rx_data_
	User recieved data.

ros::Time	transition_timestamp_
	Time at which to start transition.

std::vector< double >	safety_kp_
	PD gain when in safety mode.

std::vector< double >	safety_kd_

std::vector< double >	sit_kp_
	PD gain when in sit mode.

std::vector< double >	sit_kd_

std::vector< double >	stand_kp_
	PD gain when in standing mode.

std::vector< double >	stand_kd_

std::vector< double >	stance_kp_
	PD gain when foot is in stance.

std::vector< double >	stance_kd_

std::vector< double >	swing_kp_
	PD gain when foot is in swing.

std::vector< double >	swing_kd_

std::vector< double >	swing_kp_cart_
	PD gain when foot is in swing (Cartesian)

std::vector< double >	swing_kd_cart_

std::vector< double >	stand_joint_angles_
	Define standing joint angles.

std::vector< double >	sit_joint_angles_
	Define sitting joint angles.

std::shared_ptr< quad_utils::QuadKD >	quadKD_
	QuadKD class.

std::shared_ptr< LegController >	leg_controller_
	Leg Controller template class.

std::shared_ptr< StateEstimator >	state_estimator_
	State Estimator template class.

std::shared_ptr< HardwareInterface >	hardware_interface_
	Mblink converter object.

geometry_msgs::PoseStamped::ConstPtr	last_mocap_msg_
	Last mocap data.

sensor_msgs::Imu	last_imu_msg_
	Most recent IMU data.

sensor_msgs::JointState	last_joint_state_msg_
	Most recent joint data.

Eigen::Vector3d	vel_estimate_
	Best estimate of velocity.

Eigen::Vector3d	mocap_vel_estimate_
	Best estimate of velocity from mocap diff.

Eigen::Vector3d	imu_vel_estimate_
	Best estimate of imu velocity.

double	filter_time_constant_
	Velocity filter time constant.

double	filter_weight_
	Velocity filter weight.

double	mocap_dropout_threshold_

double	mocap_rate_
	Update rate of the motion capture system.

double	last_mainboard_time_
	Last mainboard time.

ros::Time	last_mocap_time_
	Last mocap time.

ros::Time	t_pub_
	Time of last publishing.

int	argc_
	Required for some hardware interfaces.

char **	argv_
	Required for some hardware interfaces.

Detailed Description

ROS-based driver to handle computation and interfacing for state and control.

RobotDriver implements a class to retrieve state information and generate leg commands to be sent to either the robot or a simulator. It may subscribe to any number of topics to determine the leg control, but will always publish a LegCommandArray message to control the robot's legs.

Constructor & Destructor Documentation

◆ RobotDriver()

RobotDriver::RobotDriver	(	ros::NodeHandle	nh,
		int	argc,
		char **	argv
	)

Constructor for RobotDriver.

Parameters

[in] nh ROS NodeHandle to publish and subscribe from

Returns: Constructed object of type RobotDriver

Member Function Documentation

◆ controlModeCallback()

void RobotDriver::controlModeCallback ( const std_msgs::UInt8::ConstPtr & msg )

private

Verifies and updates new control mode.

Parameters

[in] msg New control mode

◆ localPlanCallback()

void RobotDriver::localPlanCallback ( const quad_msgs::RobotPlan::ConstPtr & msg )

private

Callback function to handle new local plan (states and GRFs)

Parameters

[in] msg input message contining the local plan

◆ mocapCallback()

void RobotDriver::mocapCallback ( const geometry_msgs::PoseStamped::ConstPtr & msg )

private

Callback function to handle current robot pose.

Parameters

[in] msg input message contining current robot pose

◆ publishControl()

void RobotDriver::publishControl ( bool is_valid )

private

Function to publish leg command array message.

Parameters

[in] is_valid Boolean for if the command is valid (only send valid commands to the robot)

◆ remoteHeartbeatCallback()

void RobotDriver::remoteHeartbeatCallback ( const std_msgs::Header::ConstPtr & msg )

private

Callback to handle new remote heartbeat messages.

Parameters

[in] msg Remote heartbeat message

◆ robotStateCallback()

void RobotDriver::robotStateCallback ( const quad_msgs::RobotState::ConstPtr & msg )

private

Callback function to handle current robot state.

Parameters

[in] msg input message contining current robot state

◆ singleJointCommandCallback()

void RobotDriver::singleJointCommandCallback ( const geometry_msgs::Vector3::ConstPtr & msg )

private

Callback to handle new leg override commands.

Parameters

[in] msg Leg override commands

◆ trajectoryStateCallback()

void RobotDriver::trajectoryStateCallback ( const quad_msgs::RobotState::ConstPtr & msg )

private

Callback function to handle reference trajectory state.

Parameters

[in] msg input message contining reference trajectory state

Member Data Documentation

◆ mocap_dropout_threshold_

double RobotDriver::mocap_dropout_threshold_

private

Maximum time elapsed between mocap messages before committing to new message

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

robot_driver/include/robot_driver/robot_driver.h
robot_driver/src/robot_driver.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RobotDriver()

Member Function Documentation

◆ controlModeCallback()

◆ localPlanCallback()

◆ mocapCallback()

◆ publishControl()

◆ remoteHeartbeatCallback()

◆ robotStateCallback()

◆ singleJointCommandCallback()

◆ trajectoryStateCallback()

Member Data Documentation

◆ mocap_dropout_threshold_