sas
Modularised monitoring, logging, and control of robots.
Loading...
Searching...
No Matches
sas_robot_driver_gazebo.hpp
Go to the documentation of this file.
1/*
2# Copyright (c) 2026 Murilo Marques Marinho
3#
4# This file is part of sas_robot_driver_gazebo.
5#
6# sas_robot_driver_gazebo is free software: you can redistribute it and/or modify
7# it under the terms of the GNU Lesser General Public License as published by
8# the Free Software Foundation, either version 3 of the License, or
9# (at your option) any later version.
10#
11# sas_robot_driver_gazebo is distributed in the hope that it will be useful,
12# but WITHOUT ANY WARRANTY; without even the implied warranty of
13# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14# GNU Lesser General Public License for more details.
15#
16# You should have received a copy of the GNU Lesser General Public License
17# along with sas_robot_driver_gazebo. If not, see <https://www.gnu.org/licenses/>.
18#
19# ################################################################
20#
21# Author: Murilo M. Marinho, email: murilomarinho@ieee.org
22#
23# ################################################################
24# Contributors:
25# ---
26# Claude (Anthropic), C++ port of sas_robot_driver_gazebo.py
27# ################################################################
28*/
29#pragma once
30
41
42#include <memory>
43#include <mutex>
44#include <string>
45#include <unordered_map>
46#include <vector>
47
48#include <Eigen/Dense>
49#include <gz/msgs/model.pb.h>
50#include <gz/transport/Node.hh>
51#include <sas_core/sas_robot_driver.hpp>
52
53namespace sas
54{
55
61{
63 std::vector<std::string> joint_names;
67 std::string joint_states_topic;
68};
69
80class RobotDriverGazebo : public RobotDriver
81{
82private:
83 RobotDriverGazeboConfiguration configuration_;
84 gz::transport::Node node_;
85 std::vector<gz::transport::Node::Publisher>
86 joint_publishers_;
87 std::unordered_map<std::string, int> joint_index_;
88
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};
97
104 void _joint_states_callback(const gz::msgs::Model& msg);
105
113 Eigen::VectorXd _copy_if_received(const Eigen::VectorXd& values, const std::string& what) const;
114
115public:
117 RobotDriverGazebo() = delete;
118 ~RobotDriverGazebo() override = default;
119
126 RobotDriverGazebo(const std::shared_ptr<ShutdownSignaler>& shutdown_signaler,
127 const RobotDriverGazeboConfiguration& configuration);
128
134
140 Eigen::VectorXd get_joint_positions() override;
141
147 void set_target_joint_positions(const Eigen::VectorXd& target_joint_positions_rad) override;
148
154 Eigen::VectorXd get_joint_velocities() override;
155
161 Eigen::VectorXd get_joint_torques() override;
162
167 void connect() override;
168
172 void disconnect() override;
173
178 void initialize() override;
179
183 void deinitialize() override;
184};
185
186} // namespace sas
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