45#include <unordered_map>
49#include <gz/msgs/model.pb.h>
50#include <gz/transport/Node.hh>
51#include <sas_core/sas_robot_driver.hpp>
84 gz::transport::Node node_;
85 std::vector<gz::transport::Node::Publisher>
87 std::unordered_map<std::string, int> joint_index_;
89 mutable std::mutex mutex_;
90 Eigen::VectorXd positions_;
91 Eigen::VectorXd velocities_;
92 Eigen::VectorXd torques_;
93 Eigen::VectorXd limit_lower_;
94 Eigen::VectorXd limit_upper_;
95 std::vector<bool> received_;
96 bool all_received_{
false};
104 void _joint_states_callback(
const gz::msgs::Model& msg);
113 Eigen::VectorXd _copy_if_received(
const Eigen::VectorXd& values,
const std::string& what)
const;
RobotDriverGazebo(const RobotDriverGazebo &)=delete
Not copyable.
void set_target_joint_positions(const Eigen::VectorXd &target_joint_positions_rad) override
Publish target joint positions to Gazebo's joint position controllers.
Definition sas_robot_driver_gazebo.cpp:117
void deinitialize() override
Nothing to undo in Gazebo.
Definition sas_robot_driver_gazebo.cpp:171
void connect() override
Advertise the joints' command topics and subscribe to the joint-state topic.
Definition sas_robot_driver_gazebo.cpp:131
const RobotDriverGazeboConfiguration & get_configuration() const
The configuration this driver was constructed with.
Definition sas_robot_driver_gazebo.cpp:97
Eigen::VectorXd get_joint_velocities() override
The latest joint velocities from Gazebo.
Definition sas_robot_driver_gazebo.cpp:107
Eigen::VectorXd get_joint_positions() override
The latest joint positions from Gazebo.
Definition sas_robot_driver_gazebo.cpp:102
Eigen::VectorXd get_joint_torques() override
The latest joint torques from Gazebo.
Definition sas_robot_driver_gazebo.cpp:112
~RobotDriverGazebo() override=default
Destructor.
void disconnect() override
Unsubscribe from the joint-state topic.
Definition sas_robot_driver_gazebo.cpp:146
RobotDriverGazebo(const RobotDriverGazebo &)=delete
Not copyable.
RobotDriverGazebo()=delete
Needs a configuration.
void initialize() override
Wait for every joint's state and limits from Gazebo, then set the joint limits.
Definition sas_robot_driver_gazebo.cpp:151
Configuration of RobotDriverGazebo: the joints and the Gazebo topics.
Definition sas_robot_driver_gazebo.hpp:61
std::vector< std::string > joint_names
Names of the Gazebo joints to control, in the driver's order.
Definition sas_robot_driver_gazebo.hpp:63
std::string joint_states_topic
Gazebo topic of the joint states (gz.msgs.Model, from a JointStatePublisher).
Definition sas_robot_driver_gazebo.hpp:67
std::string joint_positions_topic_prefix
Prefix of the joints' command topics, which are prefix + joint name + "/0/cmd_pos".
Definition sas_robot_driver_gazebo.hpp:65