Controller to publish the robot state as ROS topics. More...
#include <franka_state_controller.h>
Public Member Functions | |
FrankaStateController ()=default | |
Creates an instance of a FrankaStateController. More... | |
bool | init (hardware_interface::RobotHW *robot_hardware, ros::NodeHandle &root_node_handle, ros::NodeHandle &controller_node_handle) override |
Initializes the controller with interfaces and publishers. More... | |
void | update (const ros::Time &time, const ros::Duration &period) override |
Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it. More... | |
Public Member Functions inherited from controller_interface::MultiInterfaceController< franka_hw::FrankaStateInterface > | |
virtual bool | init (hardware_interface::RobotHW *, ros::NodeHandle &) |
MultiInterfaceController (bool allow_optional_interfaces=false) | |
virtual | ~MultiInterfaceController () |
Public Member Functions inherited from controller_interface::ControllerBase | |
ControllerBase () | |
bool | isRunning () |
bool | isRunning () |
virtual void | starting (const ros::Time &) |
virtual void | starting (const ros::Time &) |
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 | ~ControllerBase () |
Private Member Functions | |
void | publishExternalWrench (const ros::Time &time) |
void | publishFrankaStates (const ros::Time &time) |
void | publishJointStates (const ros::Time &time) |
void | publishTransforms (const ros::Time &time) |
Private Attributes | |
std::string | arm_id_ |
std::unique_ptr< franka_hw::FrankaStateHandle > | franka_state_handle_ {} |
franka_hw::FrankaStateInterface * | franka_state_interface_ {} |
std::vector< std::string > | joint_names_ |
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > | publisher_external_wrench_ |
realtime_tools::RealtimePublisher< franka_msgs::FrankaState > | publisher_franka_states_ |
realtime_tools::RealtimePublisher< sensor_msgs::JointState > | publisher_joint_states_ |
realtime_tools::RealtimePublisher< sensor_msgs::JointState > | publisher_joint_states_desired_ |
realtime_tools::RealtimePublisher< tf2_msgs::TFMessage > | publisher_transforms_ |
franka::RobotState | robot_state_ |
uint64_t | sequence_number_ = 0 |
franka_hw::TriggerRate | trigger_publish_ |
Controller to publish the robot state as ROS topics.
Definition at line 23 of file franka_state_controller.h.
|
default |
Creates an instance of a FrankaStateController.
|
overridevirtual |
Initializes the controller with interfaces and publishers.
[in] | robot_hardware | RobotHW instance to get a franka_hw::FrankaStateInterface from. |
[in] | root_node_handle | Node handle in the controller_manager namespace. |
[in] | controller_node_handle | Node handle in the controller namespace. |
Reimplemented from controller_interface::MultiInterfaceController< franka_hw::FrankaStateInterface >.
Definition at line 138 of file franka_state_controller.cpp.
|
private |
Definition at line 445 of file franka_state_controller.cpp.
|
private |
Definition at line 239 of file franka_state_controller.cpp.
|
private |
Definition at line 397 of file franka_state_controller.cpp.
|
private |
Definition at line 430 of file franka_state_controller.cpp.
|
overridevirtual |
Reads the current robot state from the franka_hw::FrankaStateInterface and publishes it.
[in] | time | Current ROS time. |
[in] | period | Time since the last update. |
Implements controller_interface::ControllerBase.
Definition at line 228 of file franka_state_controller.cpp.
|
private |
Definition at line 56 of file franka_state_controller.h.
|
private |
Definition at line 59 of file franka_state_controller.h.
|
private |
Definition at line 58 of file franka_state_controller.h.
|
private |
Definition at line 69 of file franka_state_controller.h.
|
private |
Definition at line 65 of file franka_state_controller.h.
|
private |
Definition at line 62 of file franka_state_controller.h.
|
private |
Definition at line 63 of file franka_state_controller.h.
|
private |
Definition at line 64 of file franka_state_controller.h.
|
private |
Definition at line 61 of file franka_state_controller.h.
|
private |
Definition at line 67 of file franka_state_controller.h.
|
private |
Definition at line 68 of file franka_state_controller.h.
|
private |
Definition at line 66 of file franka_state_controller.h.