Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
71 changes: 35 additions & 36 deletions include/sas_robot_driver/sas_robot_driver_client.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,6 @@
#include <sas_msgs/msg/bool.hpp>


using namespace rclcpp;

namespace sas
{
Expand All @@ -69,30 +68,30 @@ class RobotDriverClient: private sas::Object
WATCHDOG_CONTROL
};
private:
std::shared_ptr<Node> node_;
std::shared_ptr<rclcpp::Node> node_;

std::vector<MODE_BLACKLIST_FLAG> blacklisted_modes_;
std::atomic_bool enabled_;
std::string topic_prefix_;

Subscription<sensor_msgs::msg::JointState>::SharedPtr subscriber_joint_states_;
VectorXd joint_positions_;
VectorXd joint_velocities_;
VectorXd joint_forces_;
Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_joint_limits_min_;
VectorXd joint_limits_min_;
Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_joint_limits_max_;
VectorXd joint_limits_max_;
Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_home_state_;
VectorXi home_states_;

Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_positions_;
Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_velocities_;
Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_forces_;
Publisher<std_msgs::msg::Int32MultiArray> ::SharedPtr publisher_homing_signal_;
Publisher<std_msgs::msg::Int32MultiArray> ::SharedPtr publisher_clear_positions_signal_;
Publisher<sas_msgs::msg::WatchdogTrigger> ::SharedPtr publisher_watchdog_trigger_;
Publisher<sas_msgs::msg::Bool> ::SharedPtr publisher_shutdown_signal_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscriber_joint_states_;
Eigen::VectorXd joint_positions_;
Eigen::VectorXd joint_velocities_;
Eigen::VectorXd joint_forces_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_joint_limits_min_;
Eigen::VectorXd joint_limits_min_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_joint_limits_max_;
Eigen::VectorXd joint_limits_max_;
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_home_state_;
Eigen::VectorXi home_states_;

rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_positions_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_velocities_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_target_joint_forces_;
rclcpp::Publisher<std_msgs::msg::Int32MultiArray> ::SharedPtr publisher_homing_signal_;
rclcpp::Publisher<std_msgs::msg::Int32MultiArray> ::SharedPtr publisher_clear_positions_signal_;
rclcpp::Publisher<sas_msgs::msg::WatchdogTrigger> ::SharedPtr publisher_watchdog_trigger_;
rclcpp::Publisher<sas_msgs::msg::Bool> ::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);
Expand All @@ -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> &node,
RobotDriverClient(const std::shared_ptr<rclcpp::Node> &node,
const std::string topic_prefix="GET_FROM_NODE",
const std::vector<MODE_BLACKLIST_FLAG>& blacklisted_modes = std::vector<MODE_BLACKLIST_FLAG>{});

Expand All @@ -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.
Expand All @@ -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<VectorXd, VectorXd> Pair of vectors (min_limits, max_limits).
* @return std::tuple<Eigen::VectorXd, Eigen::VectorXd> Pair of vectors (min_limits, max_limits).
*/
std::tuple<VectorXd, VectorXd> get_joint_limits() const;
std::tuple<Eigen::VectorXd, Eigen::VectorXd> 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.
Expand Down
7 changes: 3 additions & 4 deletions include/sas_robot_driver/sas_robot_driver_ros.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@
#include <sas_core/sas_robot_driver.hpp>
#include <sas_robot_driver/sas_robot_driver_server.hpp>

using namespace rclcpp;

namespace sas
{
Expand Down Expand Up @@ -82,7 +81,7 @@ struct RobotDriverROSConfiguration
class RobotDriverROS
{
private:
std::shared_ptr<Node> node_;
std::shared_ptr<rclcpp::Node> node_;

RobotDriverROSConfiguration configuration_;
std::atomic_bool* kill_this_node_; //Deprecated
Expand All @@ -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>& node,
RobotDriverROS(std::shared_ptr<rclcpp::Node>& node,
const std::shared_ptr<RobotDriver>& robot_driver,
const RobotDriverROSConfiguration& configuration,
const std::shared_ptr<ShutdownSignaler>& shutdown_signaler_);

[[deprecated("Use RobotDriver(const std::shared_ptr<ShutdownSignaler>& shutdown_signaler_) instead.")]]
RobotDriverROS(std::shared_ptr<Node>& node,
RobotDriverROS(std::shared_ptr<rclcpp::Node>& node,
const std::shared_ptr<RobotDriver>& robot_driver,
const RobotDriverROSConfiguration& configuration,
std::atomic_bool* kill_this_node);
Expand Down
67 changes: 33 additions & 34 deletions include/sas_robot_driver/sas_robot_driver_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,6 @@
#include <sas_msgs/msg/watchdog_trigger.hpp>
#include <sas_msgs/msg/bool.hpp>

using namespace rclcpp;

namespace sas
{
Expand All @@ -58,30 +57,30 @@ namespace sas
class RobotDriverServer: private sas::Object
{
private:
std::shared_ptr<Node> node_;
std::shared_ptr<rclcpp::Node> node_;

std::string node_prefix_;
RobotDriver::Functionality currently_active_functionality_;

Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_joint_states_;
Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_joint_limits_min_;
Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_joint_limits_max_;
Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr publisher_home_state_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_joint_states_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_joint_limits_min_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_joint_limits_max_;
rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr publisher_home_state_;


Subscription<sas_msgs::msg::Bool>::SharedPtr subscriber_shutdown_signal_;
rclcpp::Subscription<sas_msgs::msg::Bool>::SharedPtr subscriber_shutdown_signal_;
bool shutdown_signal_;
Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_positions_;
VectorXd target_joint_positions_;
Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_velocities_;
VectorXd target_joint_velocities_;
Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_forces_;
VectorXd target_joint_forces_;
Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_homing_signal_;
VectorXi homing_signal_;
Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_clear_positions_signal_;
VectorXi clear_positions_signal_;
Subscription<sas_msgs::msg::WatchdogTrigger>::SharedPtr subscriber_watchdog_trigger_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_positions_;
Eigen::VectorXd target_joint_positions_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_velocities_;
Eigen::VectorXd target_joint_velocities_;
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr subscriber_target_joint_forces_;
Eigen::VectorXd target_joint_forces_;
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_homing_signal_;
Eigen::VectorXi homing_signal_;
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr subscriber_clear_positions_signal_;
Eigen::VectorXi clear_positions_signal_;
rclcpp::Subscription<sas_msgs::msg::WatchdogTrigger>::SharedPtr subscriber_watchdog_trigger_;
bool watchdog_trigger_status_;
bool watchdog_enabled_;
double watchdog_period_in_seconds_;
Expand All @@ -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> &node, const std::string& node_prefix="GET_FROM_NODE");
RobotDriverServer(const std::shared_ptr<rclcpp::Node> &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.
Expand All @@ -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<VectorXd, VectorXd>& joint_limits);
void send_joint_limits(const std::tuple<Eigen::VectorXd, Eigen::VectorXd>& 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.
Expand Down
Loading
Loading