From 05c17f2f5557c22fe005ec2501056cade02ad690 Mon Sep 17 00:00:00 2001 From: Murilo Marinho Date: Thu, 24 Sep 2026 18:03:42 +0100 Subject: [PATCH] Remove 'using namespace rclcpp'/'Eigen' usings; qualify types Public headers (client/server/ros) no longer leak 'using namespace rclcpp;' or Eigen types (previously inherited transitively from the sas_core compatibility shims). Node/Subscription/Publisher are qualified with rclcpp:: and VectorXd/VectorXi with Eigen::. The core 'Clock' in sas_robot_driver_ros.hpp is intentionally left unqualified: it is marinholab::sas::core::Clock, not rclcpp::Clock. The Eigen qualification depends on the companion sas_cpp change that stops the public headers from leaking 'using namespace Eigen;'. This PR was created by an AI agent (OpenHands) on behalf of the repository owner. --- .../sas_robot_driver_client.hpp | 71 +++++++++---------- .../sas_robot_driver/sas_robot_driver_ros.hpp | 7 +- .../sas_robot_driver_server.hpp | 67 +++++++++-------- src/sas_robot_driver_client.cpp | 22 +++--- src/sas_robot_driver_ros.cpp | 8 +-- src/sas_robot_driver_ros_composer.cpp | 16 ++--- src/sas_robot_driver_ros_composer.hpp | 13 ++-- src/sas_robot_driver_server.cpp | 34 ++++----- 8 files changed, 117 insertions(+), 121 deletions(-) diff --git a/include/sas_robot_driver/sas_robot_driver_client.hpp b/include/sas_robot_driver/sas_robot_driver_client.hpp index 38292c7..511ac0e 100644 --- a/include/sas_robot_driver/sas_robot_driver_client.hpp +++ b/include/sas_robot_driver/sas_robot_driver_client.hpp @@ -43,7 +43,6 @@ #include -using namespace rclcpp; namespace sas { @@ -69,30 +68,30 @@ class RobotDriverClient: private sas::Object WATCHDOG_CONTROL }; private: - std::shared_ptr node_; + std::shared_ptr node_; std::vector blacklisted_modes_; std::atomic_bool enabled_; std::string topic_prefix_; - Subscription::SharedPtr subscriber_joint_states_; - VectorXd joint_positions_; - VectorXd joint_velocities_; - VectorXd joint_forces_; - Subscription::SharedPtr subscriber_joint_limits_min_; - VectorXd joint_limits_min_; - Subscription::SharedPtr subscriber_joint_limits_max_; - VectorXd joint_limits_max_; - Subscription::SharedPtr subscriber_home_state_; - VectorXi home_states_; - - Publisher::SharedPtr publisher_target_joint_positions_; - Publisher::SharedPtr publisher_target_joint_velocities_; - Publisher::SharedPtr publisher_target_joint_forces_; - Publisher ::SharedPtr publisher_homing_signal_; - Publisher ::SharedPtr publisher_clear_positions_signal_; - Publisher ::SharedPtr publisher_watchdog_trigger_; - Publisher ::SharedPtr publisher_shutdown_signal_; + rclcpp::Subscription::SharedPtr subscriber_joint_states_; + Eigen::VectorXd joint_positions_; + Eigen::VectorXd joint_velocities_; + Eigen::VectorXd joint_forces_; + rclcpp::Subscription::SharedPtr subscriber_joint_limits_min_; + Eigen::VectorXd joint_limits_min_; + rclcpp::Subscription::SharedPtr subscriber_joint_limits_max_; + Eigen::VectorXd joint_limits_max_; + rclcpp::Subscription::SharedPtr subscriber_home_state_; + Eigen::VectorXi home_states_; + + rclcpp::Publisher::SharedPtr publisher_target_joint_positions_; + rclcpp::Publisher::SharedPtr publisher_target_joint_velocities_; + rclcpp::Publisher::SharedPtr publisher_target_joint_forces_; + rclcpp::Publisher ::SharedPtr publisher_homing_signal_; + rclcpp::Publisher ::SharedPtr publisher_clear_positions_signal_; + rclcpp::Publisher ::SharedPtr publisher_watchdog_trigger_; + rclcpp::Publisher ::SharedPtr publisher_shutdown_signal_; void _callback_joint_states(const sensor_msgs::msg::JointState& msg); void _callback_joint_limits_min(const std_msgs::msg::Float64MultiArray& msg); @@ -109,7 +108,7 @@ class RobotDriverClient: private sas::Object * @param topic_prefix Topic prefix used to compose topic/service names. Defaults to "GET_FROM_NODE". * @param blacklisted_modes Optional list of modes to blacklist/disable on the client side. */ - RobotDriverClient(const std::shared_ptr &node, + RobotDriverClient(const std::shared_ptr &node, const std::string topic_prefix="GET_FROM_NODE", const std::vector& blacklisted_modes = std::vector{}); @@ -118,35 +117,35 @@ class RobotDriverClient: private sas::Object * * @param target_joint_positions Vector containing desired joint positions. */ - void send_target_joint_positions(const VectorXd& target_joint_positions); + void send_target_joint_positions(const Eigen::VectorXd& target_joint_positions); /** * @brief Publish target joint velocities. * * @param target_joint_velocities Vector containing desired joint velocities. */ - void send_target_joint_velocities(const VectorXd& target_joint_velocities); + void send_target_joint_velocities(const Eigen::VectorXd& target_joint_velocities); /** * @brief Publish target joint forces/torques. * * @param target_joint_forces Vector containing desired joint forces/torques. */ - void send_target_joint_forces(const VectorXd& target_joint_forces); + void send_target_joint_forces(const Eigen::VectorXd& target_joint_forces); /** * @brief Send a homing signal to the robot. * * @param homing_signal Vector of integers representing homing signals per joint. */ - void send_homing_signal(const VectorXi& homing_signal); + void send_homing_signal(const Eigen::VectorXi& homing_signal); /** * @brief Send a signal to clear positions on the controller side. * * @param clear_positions_signal Vector of integers representing clear position commands per joint. */ - void send_clear_positions_signal(const VectorXi& clear_positions_signal); + void send_clear_positions_signal(const Eigen::VectorXi& clear_positions_signal); /** * @brief Trigger the watchdog from the client. @@ -167,37 +166,37 @@ class RobotDriverClient: private sas::Object /** * @brief Get the last received joint positions. * - * @return VectorXd Joint positions vector. + * @return Eigen::VectorXd Joint positions vector. */ - VectorXd get_joint_positions() const; + Eigen::VectorXd get_joint_positions() const; /** * @brief Get the last received joint velocities. * - * @return VectorXd Joint velocities vector. + * @return Eigen::VectorXd Joint velocities vector. */ - VectorXd get_joint_velocities() const; + Eigen::VectorXd get_joint_velocities() const; /** * @brief Get the last received joint forces. * - * @return VectorXd Joint forces vector. + * @return Eigen::VectorXd Joint forces vector. */ - VectorXd get_joint_forces() const; + Eigen::VectorXd get_joint_forces() const; /** * @brief Retrieve the joint limits as a tuple (min, max). * - * @return std::tuple Pair of vectors (min_limits, max_limits). + * @return std::tuple Pair of vectors (min_limits, max_limits). */ - std::tuple get_joint_limits() const; + std::tuple get_joint_limits() const; /** * @brief Get the last received home states vector. * - * @return VectorXi Home states per joint. + * @return Eigen::VectorXi Home states per joint. */ - VectorXi get_home_states() const; + Eigen::VectorXi get_home_states() const; /** * @brief Check whether the client is enabled for a given functionality. diff --git a/include/sas_robot_driver/sas_robot_driver_ros.hpp b/include/sas_robot_driver/sas_robot_driver_ros.hpp index 8ef0896..67619fd 100644 --- a/include/sas_robot_driver/sas_robot_driver_ros.hpp +++ b/include/sas_robot_driver/sas_robot_driver_ros.hpp @@ -42,7 +42,6 @@ #include #include -using namespace rclcpp; namespace sas { @@ -82,7 +81,7 @@ struct RobotDriverROSConfiguration class RobotDriverROS { private: - std::shared_ptr node_; + std::shared_ptr node_; RobotDriverROSConfiguration configuration_; std::atomic_bool* kill_this_node_; //Deprecated @@ -107,13 +106,13 @@ class RobotDriverROS * @param configuration Configuration parameters for ROS integration and control loop. * @param shutdown_signaler Shared pointer used to signal shutdown between components. */ - RobotDriverROS(std::shared_ptr& node, + RobotDriverROS(std::shared_ptr& node, const std::shared_ptr& robot_driver, const RobotDriverROSConfiguration& configuration, const std::shared_ptr& shutdown_signaler_); [[deprecated("Use RobotDriver(const std::shared_ptr& shutdown_signaler_) instead.")]] - RobotDriverROS(std::shared_ptr& node, + RobotDriverROS(std::shared_ptr& node, const std::shared_ptr& robot_driver, const RobotDriverROSConfiguration& configuration, std::atomic_bool* kill_this_node); diff --git a/include/sas_robot_driver/sas_robot_driver_server.hpp b/include/sas_robot_driver/sas_robot_driver_server.hpp index 570331d..d4940f6 100644 --- a/include/sas_robot_driver/sas_robot_driver_server.hpp +++ b/include/sas_robot_driver/sas_robot_driver_server.hpp @@ -42,7 +42,6 @@ #include #include -using namespace rclcpp; namespace sas { @@ -58,30 +57,30 @@ namespace sas class RobotDriverServer: private sas::Object { private: - std::shared_ptr node_; + std::shared_ptr node_; std::string node_prefix_; RobotDriver::Functionality currently_active_functionality_; - Publisher::SharedPtr publisher_joint_states_; - Publisher::SharedPtr publisher_joint_limits_min_; - Publisher::SharedPtr publisher_joint_limits_max_; - Publisher::SharedPtr publisher_home_state_; + rclcpp::Publisher::SharedPtr publisher_joint_states_; + rclcpp::Publisher::SharedPtr publisher_joint_limits_min_; + rclcpp::Publisher::SharedPtr publisher_joint_limits_max_; + rclcpp::Publisher::SharedPtr publisher_home_state_; - Subscription::SharedPtr subscriber_shutdown_signal_; + rclcpp::Subscription::SharedPtr subscriber_shutdown_signal_; bool shutdown_signal_; - Subscription::SharedPtr subscriber_target_joint_positions_; - VectorXd target_joint_positions_; - Subscription::SharedPtr subscriber_target_joint_velocities_; - VectorXd target_joint_velocities_; - Subscription::SharedPtr subscriber_target_joint_forces_; - VectorXd target_joint_forces_; - Subscription::SharedPtr subscriber_homing_signal_; - VectorXi homing_signal_; - Subscription::SharedPtr subscriber_clear_positions_signal_; - VectorXi clear_positions_signal_; - Subscription::SharedPtr subscriber_watchdog_trigger_; + rclcpp::Subscription::SharedPtr subscriber_target_joint_positions_; + Eigen::VectorXd target_joint_positions_; + rclcpp::Subscription::SharedPtr subscriber_target_joint_velocities_; + Eigen::VectorXd target_joint_velocities_; + rclcpp::Subscription::SharedPtr subscriber_target_joint_forces_; + Eigen::VectorXd target_joint_forces_; + rclcpp::Subscription::SharedPtr subscriber_homing_signal_; + Eigen::VectorXi homing_signal_; + rclcpp::Subscription::SharedPtr subscriber_clear_positions_signal_; + Eigen::VectorXi clear_positions_signal_; + rclcpp::Subscription::SharedPtr subscriber_watchdog_trigger_; bool watchdog_trigger_status_; bool watchdog_enabled_; double watchdog_period_in_seconds_; @@ -106,52 +105,52 @@ class RobotDriverServer: private sas::Object * @param node Shared pointer to the rclcpp::Node used for communication. * @param node_prefix Prefix used to compose ROS topic names (defaults to "GET_FROM_NODE"). */ - RobotDriverServer(const std::shared_ptr &node, const std::string& node_prefix="GET_FROM_NODE"); + RobotDriverServer(const std::shared_ptr &node, const std::string& node_prefix="GET_FROM_NODE"); /** * @brief Get the most recent target joint positions received from clients. * - * @return VectorXd Target joint positions vector. + * @return Eigen::VectorXd Target joint positions vector. * @throws std::runtime_error if the server is not enabled for PositionControl * or the requested vector is uninitialized. */ - VectorXd get_target_joint_positions() const; + Eigen::VectorXd get_target_joint_positions() const; /** * @brief Get the most recent target joint velocities received from clients. * - * @return VectorXd Target joint velocities vector. + * @return Eigen::VectorXd Target joint velocities vector. * @throws std::runtime_error if the server is not enabled for VelocityControl * or the requested vector is uninitialized. */ - VectorXd get_target_joint_velocities() const; + Eigen::VectorXd get_target_joint_velocities() const; /** * @brief Get the most recent target joint forces received from clients. * - * @return VectorXd Target joint forces vector. + * @return Eigen::VectorXd Target joint forces vector. * @throws std::runtime_error if the server is not enabled for ForceControl * or the requested vector is uninitialized. */ - VectorXd get_target_joint_forces() const; + Eigen::VectorXd get_target_joint_forces() const; /** * @brief Get the last received homing signal vector. * - * @return VectorXi Homing signal per joint. + * @return Eigen::VectorXi Homing signal per joint. * @throws std::runtime_error if the server is not enabled for Homing * or the homing vector is uninitialized. */ - VectorXi get_homing_signal() const; + Eigen::VectorXi get_homing_signal() const; /** * @brief Get the last received clear positions signal vector. * - * @return VectorXi Clear positions signal per joint. + * @return Eigen::VectorXi Clear positions signal per joint. * @throws std::runtime_error if the server is not enabled for ClearPositions * or the clear positions vector is uninitialized. */ - VectorXi get_clear_positions_signal(); + Eigen::VectorXi get_clear_positions_signal(); /** * @brief Get the currently active functionality of the robot driver. @@ -176,23 +175,23 @@ class RobotDriverServer: private sas::Object * @param joint_velocities Vector of joint velocities. * @param joint_forces Vector of joint forces. */ - void send_joint_states(const VectorXd& joint_positions, - const VectorXd& joint_velocities, - const VectorXd& joint_forces); + void send_joint_states(const Eigen::VectorXd& joint_positions, + const Eigen::VectorXd& joint_velocities, + const Eigen::VectorXd& joint_forces); /** * @brief Publish joint limits (min and max) to clients. * * @param joint_limits Tuple containing (min_limits, max_limits). */ - void send_joint_limits(const std::tuple& joint_limits); + void send_joint_limits(const std::tuple& joint_limits); /** * @brief Publish the home state vector to clients. * * @param home_state Vector representing home states per joint. */ - void send_home_state(const VectorXi& home_state); + void send_home_state(const Eigen::VectorXi& home_state); /** * @brief Get the time point received from the client for watchdog synchronization. diff --git a/src/sas_robot_driver_client.cpp b/src/sas_robot_driver_client.cpp index f6acebd..65eed88 100755 --- a/src/sas_robot_driver_client.cpp +++ b/src/sas_robot_driver_client.cpp @@ -83,7 +83,7 @@ bool mode_in_blacklist(const RobotDriverClient::MODE_BLACKLIST_FLAG& mode, return std::count(list_of_flags.begin(), list_of_flags.end(), mode) > 0; } -RobotDriverClient::RobotDriverClient(const std::shared_ptr &node, +RobotDriverClient::RobotDriverClient(const std::shared_ptr &node, const std::string topic_prefix, const std::vector& blacklisted_modes): sas::Object("sas::RobotDriverClient"), @@ -126,7 +126,7 @@ RobotDriverClient::RobotDriverClient(const std::shared_ptr &node, publisher_shutdown_signal_ = node->create_publisher(topic_prefix + "/set/shutdown", 1); } -void RobotDriverClient::send_target_joint_positions(const VectorXd &target_joint_positions) +void RobotDriverClient::send_target_joint_positions(const Eigen::VectorXd &target_joint_positions) { if (publisher_target_joint_positions_) @@ -139,7 +139,7 @@ void RobotDriverClient::send_target_joint_positions(const VectorXd &target_joint throw std::runtime_error("RobotDriverClient::"+std::string(__FUNCTION__)+"::This method is blacklisted"); } -void RobotDriverClient::send_target_joint_velocities(const VectorXd &target_joint_velocities) +void RobotDriverClient::send_target_joint_velocities(const Eigen::VectorXd &target_joint_velocities) { if(publisher_target_joint_velocities_) { @@ -152,7 +152,7 @@ void RobotDriverClient::send_target_joint_velocities(const VectorXd &target_join } -void RobotDriverClient::send_target_joint_forces(const VectorXd &target_joint_efforts) +void RobotDriverClient::send_target_joint_forces(const Eigen::VectorXd &target_joint_efforts) { if (publisher_target_joint_forces_) @@ -165,7 +165,7 @@ void RobotDriverClient::send_target_joint_forces(const VectorXd &target_joint_ef throw std::runtime_error("RobotDriverClient::"+std::string(__FUNCTION__)+"::This method is blacklisted"); } -void RobotDriverClient::send_homing_signal(const VectorXi &homing_signal) +void RobotDriverClient::send_homing_signal(const Eigen::VectorXi &homing_signal) { if (publisher_homing_signal_) { @@ -177,7 +177,7 @@ void RobotDriverClient::send_homing_signal(const VectorXi &homing_signal) throw std::runtime_error("RobotDriverClient::"+std::string(__FUNCTION__)+"::This method is blacklisted"); } -void RobotDriverClient::send_clear_positions_signal(const VectorXi &clear_positions_signal) +void RobotDriverClient::send_clear_positions_signal(const Eigen::VectorXi &clear_positions_signal) { if (publisher_clear_positions_signal_) { @@ -226,7 +226,7 @@ void RobotDriverClient::send_shutdown_signal() publisher_shutdown_signal_->publish(ros_msg); } -VectorXd RobotDriverClient::get_joint_positions() const +Eigen::VectorXd RobotDriverClient::get_joint_positions() const { if(is_enabled()) return joint_positions_; @@ -234,7 +234,7 @@ VectorXd RobotDriverClient::get_joint_positions() const throw std::runtime_error(topic_prefix_ + "::RobotDriverInterface::get_joint_positions()::trying to get joint positions but uninitialized."); } -VectorXd RobotDriverClient::get_joint_velocities() const +Eigen::VectorXd RobotDriverClient::get_joint_velocities() const { if(is_enabled(RobotDriver::Functionality::VelocityControl)) return joint_velocities_; @@ -242,7 +242,7 @@ VectorXd RobotDriverClient::get_joint_velocities() const throw std::runtime_error(topic_prefix_ + "::RobotDriverInterface::get_joint_velocities()::trying to get joint velocities but uninitialized."); } -VectorXd RobotDriverClient::get_joint_forces() const +Eigen::VectorXd RobotDriverClient::get_joint_forces() const { if(is_enabled(RobotDriver::Functionality::ForceControl)) return joint_forces_; @@ -250,7 +250,7 @@ VectorXd RobotDriverClient::get_joint_forces() const throw std::runtime_error(topic_prefix_ + "::RobotDriverInterface::get_joint_efforts()::trying to get joint efforts but uninitialized."); } -std::tuple RobotDriverClient::get_joint_limits() const +std::tuple RobotDriverClient::get_joint_limits() const { if(is_enabled()) { @@ -260,7 +260,7 @@ std::tuple RobotDriverClient::get_joint_limits() const throw std::runtime_error(topic_prefix_ + "::RobotDriverInterface::get_joint_limits()::trying to get joint limits but uninitialized."); } -VectorXi RobotDriverClient::get_home_states() const +Eigen::VectorXi RobotDriverClient::get_home_states() const { if(is_enabled(RobotDriver::Functionality::Homing)) { diff --git a/src/sas_robot_driver_ros.cpp b/src/sas_robot_driver_ros.cpp index 14ccbe9..5bbaf92 100644 --- a/src/sas_robot_driver_ros.cpp +++ b/src/sas_robot_driver_ros.cpp @@ -49,7 +49,7 @@ bool are_approximately_equal(const double &a, const double &b, const double &eps namespace sas { -RobotDriverROS::RobotDriverROS(std::shared_ptr &node, +RobotDriverROS::RobotDriverROS(std::shared_ptr &node, const std::shared_ptr &robot_driver, const RobotDriverROSConfiguration &configuration, std::atomic_bool *kill_this_node): @@ -65,7 +65,7 @@ RobotDriverROS::RobotDriverROS(std::shared_ptr &node, } -RobotDriverROS::RobotDriverROS(std::shared_ptr &node, +RobotDriverROS::RobotDriverROS(std::shared_ptr &node, const std::shared_ptr &robot_driver, const RobotDriverROSConfiguration &configuration, const std::shared_ptr& shutdown_signaler): @@ -194,9 +194,9 @@ int RobotDriverROS::control_loop() auto joint_positions{robot_driver_->get_joint_positions()}; - VectorXd joint_velocities; + Eigen::VectorXd joint_velocities; try{joint_velocities = robot_driver_->get_joint_velocities();} catch(...){} - VectorXd joint_torques; + Eigen::VectorXd joint_torques; try{joint_torques = robot_driver_->get_joint_torques();} catch(...){} robot_driver_server_.send_joint_states(joint_positions, joint_velocities, joint_torques); diff --git a/src/sas_robot_driver_ros_composer.cpp b/src/sas_robot_driver_ros_composer.cpp index 5f8ea79..5be9c1d 100755 --- a/src/sas_robot_driver_ros_composer.cpp +++ b/src/sas_robot_driver_ros_composer.cpp @@ -33,7 +33,7 @@ namespace sas { RobotDriverROSComposer::RobotDriverROSComposer(const RobotDriverROSComposerConfiguration &configuration, - std::shared_ptr &node, + std::shared_ptr &node, std::atomic_bool *break_loops): RobotDriver(break_loops), node_(node), @@ -53,10 +53,10 @@ RobotDriverROSComposer::RobotDriverROSComposer(const RobotDriverROSComposerConfi } } -VectorXd RobotDriverROSComposer::get_joint_positions() +Eigen::VectorXd RobotDriverROSComposer::get_joint_positions() { - VectorXd joint_positions; + Eigen::VectorXd joint_positions; for(const auto& interface : robot_driver_clients_) { joint_positions = concatenate(joint_positions, interface->get_joint_positions()); @@ -64,7 +64,7 @@ VectorXd RobotDriverROSComposer::get_joint_positions() return joint_positions; } -void RobotDriverROSComposer::set_target_joint_positions(const VectorXd &set_target_joint_positions_rad) +void RobotDriverROSComposer::set_target_joint_positions(const Eigen::VectorXd &set_target_joint_positions_rad) { int accumulator = 0; for(const auto& interface : robot_driver_clients_) @@ -74,7 +74,7 @@ void RobotDriverROSComposer::set_target_joint_positions(const VectorXd &set_targ } } -void RobotDriverROSComposer::set_joint_limits(const std::tuple&) +void RobotDriverROSComposer::set_joint_limits(const std::tuple&) { throw std::runtime_error("RobotDriverROSComposer::set_joint_limits::Not accepted."); } @@ -112,12 +112,12 @@ void RobotDriverROSComposer::deinitialize() RobotDriverROSComposer::~RobotDriverROSComposer()=default; -std::tuple RobotDriverROSComposer::get_joint_limits() +std::tuple RobotDriverROSComposer::get_joint_limits() { if(!configuration_.override_joint_limits_with_robot_parameter_file) { - VectorXd joint_positions_min; - VectorXd joint_positions_max; + Eigen::VectorXd joint_positions_min; + Eigen::VectorXd joint_positions_max; for(const auto& interface : robot_driver_clients_) { auto [joint_positions_min_l, joint_positions_max_l] = interface->get_joint_limits(); diff --git a/src/sas_robot_driver_ros_composer.hpp b/src/sas_robot_driver_ros_composer.hpp index 04b68f7..9de1f17 100755 --- a/src/sas_robot_driver_ros_composer.hpp +++ b/src/sas_robot_driver_ros_composer.hpp @@ -34,7 +34,6 @@ #include #include -using namespace Eigen; namespace sas { @@ -49,7 +48,7 @@ struct RobotDriverROSComposerConfiguration class RobotDriverROSComposer: public RobotDriver { protected: - std::shared_ptr node_; + std::shared_ptr node_; RobotDriverROSComposerConfiguration configuration_; std::vector> robot_driver_clients_; @@ -58,13 +57,13 @@ class RobotDriverROSComposer: public RobotDriver RobotDriverROSComposer(const RobotDriverROSComposer&)=delete; public: RobotDriverROSComposer(const RobotDriverROSComposerConfiguration& configuration, - std::shared_ptr& node, + std::shared_ptr& node, std::atomic_bool *break_loops); - VectorXd get_joint_positions() override; - void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) override; - std::tuple get_joint_limits() override; - void set_joint_limits(const std::tuple&) override; + Eigen::VectorXd get_joint_positions() override; + void set_target_joint_positions(const Eigen::VectorXd& set_target_joint_positions_rad) override; + std::tuple get_joint_limits() override; + void set_joint_limits(const std::tuple&) override; void connect() override; void disconnect() override; diff --git a/src/sas_robot_driver_server.cpp b/src/sas_robot_driver_server.cpp index dad14bb..e79a42d 100755 --- a/src/sas_robot_driver_server.cpp +++ b/src/sas_robot_driver_server.cpp @@ -90,7 +90,7 @@ void RobotDriverServer::_callback_homing_signal(const std_msgs::msg::Int32MultiA */ void RobotDriverServer::_callback_clear_positions_signal(const std_msgs::msg::Int32MultiArray& msg) { - VectorXi clear_positions_signal_temp(msg.data.size()); + Eigen::VectorXi clear_positions_signal_temp(msg.data.size()); //We keep the clear position flags as 1 until they are processed by get_clear_positions_signal() for(int i=0;i &node, const std::string &topic_prefix): +RobotDriverServer::RobotDriverServer(const std::shared_ptr &node, const std::string &topic_prefix): sas::Object("sas::RobotDriverServer"), node_(node), node_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix), @@ -164,7 +164,7 @@ RobotDriverServer::RobotDriverServer(const std::shared_ptr &node, const st ); } -VectorXd RobotDriverServer::get_target_joint_positions() const +Eigen::VectorXd RobotDriverServer::get_target_joint_positions() const { if(is_enabled(RobotDriver::Functionality::PositionControl)) return target_joint_positions_; @@ -172,7 +172,7 @@ VectorXd RobotDriverServer::get_target_joint_positions() const throw std::runtime_error(node_prefix_ + "::RobotDriverProvider::get_target_joint_positions() trying to get an uninitialized vector"); } -VectorXd RobotDriverServer::get_target_joint_velocities() const +Eigen::VectorXd RobotDriverServer::get_target_joint_velocities() const { if(is_enabled(RobotDriver::Functionality::VelocityControl)) return target_joint_velocities_; @@ -180,7 +180,7 @@ VectorXd RobotDriverServer::get_target_joint_velocities() const throw std::runtime_error(node_prefix_ + "::RobotDriverProvider::get_target_joint_velocities() trying to get an uninitialized vector"); } -VectorXd RobotDriverServer::get_target_joint_forces() const +Eigen::VectorXd RobotDriverServer::get_target_joint_forces() const { if(is_enabled(RobotDriver::Functionality::ForceControl)) return target_joint_forces_; @@ -190,9 +190,9 @@ VectorXd RobotDriverServer::get_target_joint_forces() const /** * @brief get_homing_signal - * @return a VectorXi with 1s for the joints that should be homed and 0s for the joints that should not be homed. + * @return a Eigen::VectorXi with 1s for the joints that should be homed and 0s for the joints that should not be homed. */ -VectorXi RobotDriverServer::get_homing_signal() const +Eigen::VectorXi RobotDriverServer::get_homing_signal() const { if(is_enabled(RobotDriver::Functionality::Homing)) return homing_signal_; @@ -202,14 +202,14 @@ VectorXi RobotDriverServer::get_homing_signal() const /** * @brief RobotDriverProvider::get_clear_positions_signal. Getting the clear positions signal also clears it. - * @return a VectorXi with 0s for configurations that should not be cleared and 1 for positions that should be cleared. + * @return a Eigen::VectorXi with 0s for configurations that should not be cleared and 1 for positions that should be cleared. */ -VectorXi RobotDriverServer::get_clear_positions_signal() +Eigen::VectorXi RobotDriverServer::get_clear_positions_signal() { if(is_enabled(RobotDriver::Functionality::ClearPositions)) { - const VectorXi return_value(clear_positions_signal_); - clear_positions_signal_ = VectorXi::Zero(return_value.size()); + const Eigen::VectorXi return_value(clear_positions_signal_); + clear_positions_signal_ = Eigen::VectorXi::Zero(return_value.size()); return return_value; } else @@ -223,11 +223,11 @@ RobotDriver::Functionality RobotDriverServer::get_currently_active_functionality /** * @brief Sends the current joint states through ROS. - * @param joint_positions vector of . If not needed, use joint_positions=VectorXd(). - * @param joint_velocities. If not needed, use joint_velocities=VectorXd(). - * @param joint_forces. If not needed, use joint_forces=VectorXd(). + * @param joint_positions vector of . If not needed, use joint_positions=Eigen::VectorXd(). + * @param joint_velocities. If not needed, use joint_velocities=Eigen::VectorXd(). + * @param joint_forces. If not needed, use joint_forces=Eigen::VectorXd(). */ -void RobotDriverServer::send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces) +void RobotDriverServer::send_joint_states(const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const Eigen::VectorXd &joint_forces) { sensor_msgs::msg::JointState ros_msg; ros_msg.header.stamp = node_->get_clock()->now(); @@ -240,7 +240,7 @@ void RobotDriverServer::send_joint_states(const VectorXd &joint_positions, const publisher_joint_states_->publish(ros_msg); } -void RobotDriverServer::send_joint_limits(const std::tuple &joint_limits) +void RobotDriverServer::send_joint_limits(const std::tuple &joint_limits) { std_msgs::msg::Float64MultiArray ros_msg_min; ros_msg_min.data = vectorxd_to_std_vector_double(std::get<0>(joint_limits)); @@ -251,7 +251,7 @@ void RobotDriverServer::send_joint_limits(const std::tuple & publisher_joint_limits_max_->publish(ros_msg_max); } -void RobotDriverServer::send_home_state(const VectorXi &home_state) +void RobotDriverServer::send_home_state(const Eigen::VectorXi &home_state) { std_msgs::msg::Int32MultiArray ros_msg_home_state; ros_msg_home_state.data = vectorxi_to_std_vector_int(home_state);