From 06d4fe7f5ca2080f20042ee5339c9e423eee0154 Mon Sep 17 00:00:00 2001 From: Walden Leung <7531347+leungw16@users.noreply.github.com> Date: Sun, 22 Sep 2024 22:17:52 +1000 Subject: [PATCH] Changed map>> to structs and variants when storing topic and interface info --- .../vehicle_urdf/urdf/ros2_control.urdf.xacro | 21 ++ .../qev_3d_topic_interface.hpp | 104 ++++++-- .../src/qev_3d_topic_interface.cpp | 232 ++++++++++++++---- .../include/component_canbus_translator.hpp | 1 + .../src/component_canbus_translator.cpp | 14 ++ 5 files changed, 295 insertions(+), 77 deletions(-) diff --git a/src/common/vehicle_urdf/urdf/ros2_control.urdf.xacro b/src/common/vehicle_urdf/urdf/ros2_control.urdf.xacro index b0b1ff67..5f5b7279 100644 --- a/src/common/vehicle_urdf/urdf/ros2_control.urdf.xacro +++ b/src/common/vehicle_urdf/urdf/ros2_control.urdf.xacro @@ -4,6 +4,7 @@ mock_components/GenericSystem true + /vehicle/ackermann_cmd @@ -14,6 +15,7 @@ 0 + driving @@ -24,13 +26,32 @@ 0 + driving + + + + + + 0 + + /vehicle/rw_position + + + 0 + + /vehicle/velocity + + driving 0 + + /vehicle/steering_angle + steering diff --git a/src/control/ros2_control/qutms_hw_interfaces/include/qutms_hw_interfaces/qev_3d_topic_interface.hpp b/src/control/ros2_control/qutms_hw_interfaces/include/qutms_hw_interfaces/qev_3d_topic_interface.hpp index f02db52d..3ae25f83 100644 --- a/src/control/ros2_control/qutms_hw_interfaces/include/qutms_hw_interfaces/qev_3d_topic_interface.hpp +++ b/src/control/ros2_control/qutms_hw_interfaces/include/qutms_hw_interfaces/qev_3d_topic_interface.hpp @@ -16,7 +16,9 @@ #define QUTMS_HW_INTERFACES__QEV_3D_TOPIC_INTERFACE_HPP_ #include +#include #include +#include #include #include "ackermann_msgs/msg/ackermann_drive_stamped.hpp" @@ -36,16 +38,28 @@ namespace qutms_hw_interfaces { using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using MessageTypeVariant = std::variant; + +using PublisherVariant = std::variant::SharedPtr, + rclcpp::Publisher::SharedPtr, + rclcpp::Publisher::SharedPtr, + rclcpp::Publisher::SharedPtr>; + +using SubscriberVariant = std::variant::SharedPtr, + rclcpp::Subscription::SharedPtr, + rclcpp::Subscription::SharedPtr, + rclcpp::Subscription::SharedPtr>; // Structs to store joint states struct JointValue { - _Float64 position{0.0}; - _Float32 velocity{0.0}; - _Float64 acceleration{0.0}; + double position{0.0}; + double velocity{0.0}; + double acceleration{0.0}; double effort{0.0}; }; struct Joint { - explicit Joint(const std::string& name) : joint_name(name) { + explicit Joint(const std::string& name, const std::string& type) : joint_name(name), joint_type(type) { state = JointValue(); command = JointValue(); } @@ -53,9 +67,63 @@ struct Joint { Joint() = default; std::string joint_name; + std::string joint_type; + std::vector command_interfaces; + // Currently unused: For future use + std::vector> command_publishers; + std::vector state_interfaces; + std::vector> state_subscribers; JointValue state; JointValue command; }; + +class PublisherWrapper { + public: + template + PublisherWrapper(typename rclcpp::Publisher::SharedPtr publisher) : publisher_(publisher) {} + + void publish(const MessageTypeVariant& msgVariant) { + std::visit( + [this](auto& msg) { + using MsgType = std::decay_t; + if (auto pub = + std::get_if::type>::SharedPtr>( + &publisher_)) { + (*pub)->publish(msg); + } + }, + msgVariant); + } + + private: + std::variant::SharedPtr, + rclcpp::Publisher::SharedPtr, + rclcpp::Publisher::SharedPtr, + rclcpp::Publisher::SharedPtr> + publisher_; +}; + +class SubscriberWrapper { + public: + template + SubscriberWrapper(typename rclcpp::Subscription::SharedPtr subscriber) : subscriber_(std::move(subscriber)) {} + + // MessageTypeVariant handle_message(const MessageTypeVariant& msgVariant){ + // std::visit([this](auto& msg){ + // using MsgType = std::decay_t; + // if (auto sub = std::get_if::SharedPtr>(&subscriber_)){ + // return msg; + // } + // }, msgVariant); + // } + private: + std::variant::SharedPtr, + rclcpp::Subscription::SharedPtr, + rclcpp::Subscription::SharedPtr, + rclcpp::Subscription::SharedPtr> + subscriber_; +}; + class Qev3dTopicInterface : public hardware_interface::SystemInterface { public: TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC @@ -85,6 +153,7 @@ class Qev3dTopicInterface : public hardware_interface::SystemInterface { private: std::vector hw_commands_; std::vector hw_states_; + // Definition for joints that are configured for this interface // Format: // { @@ -104,12 +173,13 @@ class Qev3dTopicInterface : public hardware_interface::SystemInterface { // }, // {Joint_type(1 Unique)->{steering/drive/other}} // }), - const std::map>> available_joints = { - ("virtual_steering_hinge_joint", - {{hardware_interface::HW_IF_POSITION}, {hardware_interface::HW_IF_POSITION}, {"steering"}}), - ("virtual_rear_wheel_joint", {{hardware_interface::HW_IF_EFFORT}, - {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}, - {"drive"}})}; + // const std::map>> available_joints = { + // {"virtual_steering_hinge_joint", + // {{hardware_interface::HW_IF_POSITION}, {hardware_interface::HW_IF_POSITION}, {"steering"}}}, + // {"virtual_rear_wheel_joint", {{hardware_interface::HW_IF_EFFORT}, + // {hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY}, + // {"drive"}}}}; + const std::set available_joints = {"virtual_steering_hinge_joint", "virtual_rear_wheel_joint"}; std::map hw_interfaces_; rclcpp::Subscription::SharedPtr status_sub_; rclcpp::Subscription::SharedPtr velocity_sub_; @@ -123,20 +193,6 @@ class Qev3dTopicInterface : public hardware_interface::SystemInterface { std_msgs::msg::Float32 last_steering_angle_; std::string driving_joint_; std::string steering_joint_; - - struct config { - // Use the following for Tricycle drive: - std::string left_wheel_joint = ""; - std::string right_wheel_joint = ""; - // For Bicycle Drive: - std::string rear_wheel_joint = ""; - // Publishing and Subscription Topics - std::string ackermann_pub_topic = ""; - std::string steering_sub_topic = ""; - std::string velocity_sub_topic = ""; - std::string position_sub_topic = ""; - std::string status_sub_topic = ""; - }; }; } // namespace qutms_hw_interfaces diff --git a/src/control/ros2_control/qutms_hw_interfaces/src/qev_3d_topic_interface.cpp b/src/control/ros2_control/qutms_hw_interfaces/src/qev_3d_topic_interface.cpp index d5b49874..cd17a368 100644 --- a/src/control/ros2_control/qutms_hw_interfaces/src/qev_3d_topic_interface.cpp +++ b/src/control/ros2_control/qutms_hw_interfaces/src/qev_3d_topic_interface.cpp @@ -27,26 +27,84 @@ hardware_interface::CallbackReturn Qev3dTopicInterface::on_init(const hardware_i } // Read parameters and initialize the hardware - hw_states_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_commands_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_states_.resize(info_.joints.size(), ::std::numeric_limits::quiet_NaN()); + hw_commands_.resize(info_.joints.size(), ::std::numeric_limits::quiet_NaN()); + + // Create ros Node to publish and subscribe to topics + rclcpp::NodeOptions options; + options.arguments({"--ros-args", "-r", "__node:=qev_3d_topic_interface_" + info_.name}); + + node_ = rclcpp::Node::make_shared("_", options); // Populate valid joints as joint objects and save it in hw_interfaces_ for (const hardware_interface::ComponentInfo& joint_cfg : info_.joints) { if (available_joints.find(joint_cfg.name) != available_joints.end()) { - hw_interfaces_[joint_cfg.name] = Joint(joint_cfg.name); + hw_interfaces_[joint_cfg.name] = Joint(joint_cfg.name, joint_cfg.parameters.find("joint_type")->second); + for (auto command_interface : joint_cfg.command_interfaces) { + hw_interfaces_[joint_cfg.name].command_interfaces.push_back(*command_interface.name.c_str()); + if (command_interface.data_type != "") { + rclcpp::Publisher::SharedPtr publisher = + node_->create_publisher(command_interface.data_type, rclcpp::QoS(1)); + hw_interfaces_[joint_cfg.name].command_publishers.emplace_back(PublisherWrapper(publisher)); + } else { + hw_interfaces_[joint_cfg.name].command_publishers.emplace_back(nullptr); + } + } + for (auto state_interface : joint_cfg.state_interfaces) { + hw_interfaces_[joint_cfg.name].state_interfaces.push_back(*state_interface.name.c_str()); + // SubscriberWrapper subscriber; + if (state_interface.data_type != "") { + if (state_interface.data_type == hardware_interface::HW_IF_POSITION) { + if (joint_cfg.parameters.find("joint_type")->second == "driving") { + auto subscriber = node_->create_subscription( + state_interface.data_type, rclcpp::SensorDataQoS(), + [this, joint_cfg](const std_msgs::msg::Float64::SharedPtr msg) { + hw_interfaces_[joint_cfg.name].state.position = msg->data; + }); + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back( + SubscriberWrapper(subscriber)); + } else { + auto subscriber = node_->create_subscription( + state_interface.data_type, rclcpp::SensorDataQoS(), + [this, joint_cfg](const std_msgs::msg::Float32::SharedPtr msg) { + hw_interfaces_[joint_cfg.name].state.position = msg->data; + }); + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back( + SubscriberWrapper(subscriber)); + } + } else if (state_interface.data_type == hardware_interface::HW_IF_VELOCITY) { + auto subscriber = node_->create_subscription( + state_interface.data_type, rclcpp::SensorDataQoS(), + [this, joint_cfg](const std_msgs::msg::Float64::SharedPtr msg) { + hw_interfaces_[joint_cfg.name].state.velocity = msg->data; + }); + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back(SubscriberWrapper(subscriber)); + } else if (state_interface.data_type == hardware_interface::HW_IF_ACCELERATION) { + auto subscriber = node_->create_subscription( + state_interface.data_type, rclcpp::SensorDataQoS(), + [this, joint_cfg](const std_msgs::msg::Float64::SharedPtr msg) { + hw_interfaces_[joint_cfg.name].state.acceleration = msg->data; + }); + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back(SubscriberWrapper(subscriber)); + } else if (state_interface.data_type == hardware_interface::HW_IF_EFFORT) { + auto subscriber = node_->create_subscription( + state_interface.data_type, rclcpp::SensorDataQoS(), + [this, joint_cfg](const std_msgs::msg::Float64::SharedPtr msg) { + hw_interfaces_[joint_cfg.name].state.effort = msg->data; + }); + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back(SubscriberWrapper(subscriber)); + } + } else { + hw_interfaces_[joint_cfg.name].state_subscribers.emplace_back(nullptr); + } + } } else { RCLCPP_WARN(rclcpp::get_logger("qev_3d_topic_interface"), "Joint '%s' is not a supported interface. Skipping", joint_cfg.name.c_str()); } } - // Create ros Node to publish and subscribe to topics - rclcpp::NodeOptions options; - options.arguments({"--ros-args", "-r", "__node:=qev_3d_topic_interface_" + info_.name}); - - node_ = rclcpp::Node::make_shared("_", options); - - // Function to set publish and subscription topic based on parameter or default + // Function to set publish and subscription topic based on parameter or defaults const auto get_hardware_parameter = [this](const std::string& parameter_name, const std::string& default_value) { if (auto it = info_.hardware_parameters.find(parameter_name); it != info_.hardware_parameters.end()) { return it->second; @@ -56,24 +114,24 @@ hardware_interface::CallbackReturn Qev3dTopicInterface::on_init(const hardware_i // Setup publisher and subscribers based on configured parameters or configuration ackermann_pub_ = node_->create_publisher( - get_hardware_parameter("ackermann_topic", "/ackermann_cmd"), rclcpp::QoS(1)); + get_hardware_parameter("ackermann_topic", "vehicle/ackermann_cmd"), rclcpp::QoS(1)); status_sub_ = node_->create_subscription( get_hardware_parameter("status_topic", "/status"), rclcpp::QoS(1), [this](const driverless_msgs::msg::State::SharedPtr status) { last_status_ = *status; }); - velocity_sub_ = node_->create_subscription( - get_hardware_parameter("velocity_topic", "/velocity"), rclcpp::SensorDataQoS(), - [this](const std_msgs::msg::Float32::SharedPtr velocity) { last_velocity_ = *velocity; }); - position_sub_ = node_->create_subscription( - get_hardware_parameter("position_topic", "/position"), rclcpp::SensorDataQoS(), - [this](const std_msgs::msg::Float64::SharedPtr position) { last_position_ = *position; }); - steering_angle_sub_ = node_->create_subscription( - get_hardware_parameter("steering_angle_topic", "/steering_angle"), rclcpp::SensorDataQoS(), - [this](const std_msgs::msg::Float32::SharedPtr steering_angle) { last_steering_angle_ = *steering_angle; }); + // velocity_sub_ = node_->create_subscription( + // get_hardware_parameter("velocity_topic", "/velocity"), rclcpp::SensorDataQoS(), + // [this](const std_msgs::msg::Float32::SharedPtr velocity) { last_velocity_ = *velocity; }); + // position_sub_ = node_->create_subscription( + // get_hardware_parameter("position_topic", "/position"), rclcpp::SensorDataQoS(), + // [this](const std_msgs::msg::Float64::SharedPtr position) { last_position_ = *position; }); + // steering_angle_sub_ = node_->create_subscription( + // get_hardware_parameter("steering_angle_topic", "/steering_angle"), rclcpp::SensorDataQoS(), + // [this](const std_msgs::msg::Float32::SharedPtr steering_angle) { last_steering_angle_ = *steering_angle; }); // Get Driving joint name (Bicycle) - driving_joint_ = get_hardware_parameter("driving_joint", "virtual_rear_wheel_joint"); + // driving_joint_ = get_hardware_parameter("driving_joint", "virtual_rear_wheel_joint"); // Get Steering joint name (Bicycle) - steering_joint_ = get_hardware_parameter("steering_joint", "virtual_steering_hinge_joint"); + // steering_joint_ = get_hardware_parameter("steering_joint", "virtual_steering_hinge_joint"); RCLCPP_INFO(rclcpp::get_logger("qev_3d_topic_interface"), "QEV3D Hardware Interface has been initialized"); @@ -90,22 +148,42 @@ hardware_interface::CallbackReturn Qev3dTopicInterface::on_configure( std::vector Qev3dTopicInterface::export_state_interfaces() { std::vector state_interfaces; for (auto& joint : hw_interfaces_) { - for (auto& interface : available_joints.find(joint.second.joint_name)[1]) { - if (interface == hardware_interface::HW_IF_POSITION) { - state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, interface, - &joint.second.state.position)); - } else if (interface == hardware_interface::HW_IF_VELOCITY) { - state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, interface, - &joint.second.state.velocity)); - } else if (interface == hardware_interface::HW_IF_ACCELERATION) { - state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, interface, - &joint.second.state.acceleration)); - } else if (interface == hardware_interface::HW_IF_EFFORT) { - state_interfaces.emplace_back( - hardware_interface::StateInterface(joint.second.joint_name, interface, &joint.second.state.effort)); + // for (auto& interface : available_joints.find(joint.second.joint_name)->second[1]) { + // if (&interface == hardware_interface::HW_IF_POSITION) { + // state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, &interface, + // &joint.second.state.position)); + // } else if (&interface == hardware_interface::HW_IF_VELOCITY) { + // state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, &interface, + // &joint.second.state.velocity)); + // } else if (&interface == hardware_interface::HW_IF_ACCELERATION) { + // state_interfaces.emplace_back(hardware_interface::StateInterface(joint.second.joint_name, &interface, + // &joint.second.state.acceleration)); + // } else if (&interface == hardware_interface::HW_IF_EFFORT) { + // state_interfaces.emplace_back( + // hardware_interface::StateInterface(joint.second.joint_name, &interface, + // &joint.second.state.effort)); + // } else { + // RCLCPP_WARN(rclcpp::get_logger("qev_3d_topic_interface"), + // "State interface '%d' is not supported for joint '%s'. Skipping", interface, + // joint.second.joint_name.c_str()); + // } + // } + for (char& state_interface : joint.second.state_interfaces) { + if (&state_interface == hardware_interface::HW_IF_POSITION) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_POSITION, &joint.second.state.position)); + } else if (&state_interface == hardware_interface::HW_IF_VELOCITY) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint.second.state.velocity)); + } else if (&state_interface == hardware_interface::HW_IF_ACCELERATION) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_ACCELERATION, &joint.second.state.acceleration)); + } else if (&state_interface == hardware_interface::HW_IF_EFFORT) { + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.second.joint_name, hardware_interface::HW_IF_EFFORT, &joint.second.state.effort)); } else { RCLCPP_WARN(rclcpp::get_logger("qev_3d_topic_interface"), - "State interface '%s' is not supported for joint '%s'. Skipping", interface.c_str(), + "State interface '%s' is not supported for joint '%s'. Skipping", &state_interface, joint.second.joint_name.c_str()); } } @@ -117,26 +195,49 @@ std::vector Qev3dTopicInterface::export_stat std::vector Qev3dTopicInterface::export_command_interfaces() { std::vector command_interfaces; for (auto& joint_interface : hw_interfaces_) { - for (auto& interface : available_joints.find(joint_interface.second.joint_name)[0]) { - if (interface == hardware_interface::HW_IF_POSITION) { + // for (auto& interface : available_joints.find(joint_interface.second.joint_name)->second[0]) { + // if (&interface == hardware_interface::HW_IF_POSITION) { + // command_interfaces.emplace_back(hardware_interface::CommandInterface( + // joint_interface.second.joint_name, hardware_interface::HW_IF_POSITION, + // &joint_interface.second.command.position)); + // } else if (&interface == hardware_interface::HW_IF_VELOCITY) { + // command_interfaces.emplace_back(hardware_interface::CommandInterface( + // joint_interface.second.joint_name, hardware_interface::HW_IF_VELOCITY, + // &joint_interface.second.command.velocity)); + // } else if (&interface == hardware_interface::HW_IF_ACCELERATION) { + // command_interfaces.emplace_back(hardware_interface::CommandInterface( + // joint_interface.second.joint_name, hardware_interface::HW_IF_ACCELERATION, + // &joint_interface.second.command.acceleration)); + // } else if (&interface == hardware_interface::HW_IF_EFFORT) { + // command_interfaces.emplace_back(hardware_interface::CommandInterface( + // joint_interface.second.joint_name, hardware_interface::HW_IF_EFFORT, + // &joint_interface.second.command.effort)); + // } else { + // RCLCPP_WARN(rclcpp::get_logger("qev_3d_topic_interface"), + // "Command interface '%d' is not supported for joint '%s'. Skipping", interface, + // joint_interface.second.joint_name.c_str()); + // } + // } + for (char& command_interface : joint_interface.second.command_interfaces) { + if (&command_interface == hardware_interface::HW_IF_POSITION) { command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_interface.second.joint_name, hardware_interface::HW_IF_POSITION, &joint_interface.second.command.position)); - } else if (interface == hardware_interface::HW_IF_VELOCITY) { + } else if (&command_interface == hardware_interface::HW_IF_VELOCITY) { command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_interface.second.joint_name, hardware_interface::HW_IF_VELOCITY, &joint_interface.second.command.velocity)); - } else if (interface == hardware_interface::HW_IF_ACCELERATION) { + } else if (&command_interface == hardware_interface::HW_IF_ACCELERATION) { command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_interface.second.joint_name, hardware_interface::HW_IF_ACCELERATION, &joint_interface.second.command.acceleration)); - } else if (interface == hardware_interface::HW_IF_EFFORT) { + } else if (&command_interface == hardware_interface::HW_IF_EFFORT) { command_interfaces.emplace_back(hardware_interface::CommandInterface( joint_interface.second.joint_name, hardware_interface::HW_IF_EFFORT, &joint_interface.second.command.effort)); } else { RCLCPP_WARN(rclcpp::get_logger("qev_3d_topic_interface"), - "Command interface '%s' is not supported for joint '%s'. Skipping", interface.c_str(), + "Command interface '%s' is not supported for joint '%s'. Skipping", &command_interface, joint_interface.second.joint_name.c_str()); } } @@ -164,26 +265,51 @@ hardware_interface::return_type Qev3dTopicInterface::read(const rclcpp::Time& /* rclcpp::spin_some(node_); } - for (auto& joint : hw_interfaces_) { - if (available_joints.find(joint.second.joint_name)[2] == "steering") { - joint.second.state.position = last_steering_angle_.data; - } else if (available_joints.find(joint.second.joint_name)[2] == "drive") { - joint.second.state.position = last_position_.data; - joint.second.state.velocity = last_velocity_.data; - } - } + // for (auto& joint : hw_interfaces_) { + // if (&available_joints.find(joint.second.joint_name)->second[2][0] == "steering") { + // joint.second.state.position = last_steering_angle_.data; + // } else if (&available_joints.find(joint.second.joint_name)->second[2][0] == "drive") { + // joint.second.state.position = last_position_.data; + // joint.second.state.velocity = last_velocity_.data; + // } + // } return hardware_interface::return_type::OK; } hardware_interface::return_type Qev3dTopicInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { - if (last_status_.data.state == driverless_msgs::msg::State::DRIVING) { + if (last_status_.state == driverless_msgs::msg::State::DRIVING) { ackermann_msgs::msg::AckermannDriveStamped ackermann_msg; ackermann_msg.header.stamp = node_->now(); ackermann_msg.header.frame_id = "base_footprint"; - ackermann_msg.drive.steering_angle = hw_interfaces_[steering_joint_].command.position; - ackermann_msg.drive.speed = hw_interfaces_[driving_joint_].command.effort; + ackermann_msg.drive.steering_angle = static_cast(hw_interfaces_[steering_joint_].command.position); + ackermann_msg.drive.speed = static_cast(hw_interfaces_[driving_joint_].command.effort); ackermann_pub_->publish(ackermann_msg); + + for (auto& joint : hw_interfaces_) { + for (size_t i = 0; i < joint.second.command_interfaces.size(); i++) { + int j = static_cast(i); + qutms_hw_interfaces::MessageTypeVariant msg; + if (&joint.second.command_interfaces[j] == hardware_interface::HW_IF_POSITION) { + msg = std_msgs::msg::Float64(); + std::get(msg).data = joint.second.command.position; + } else if (&joint.second.command_interfaces[j] == hardware_interface::HW_IF_VELOCITY) { + msg = std_msgs::msg::Float32(); + std::get(msg).data = joint.second.command.velocity; + } else if (&joint.second.command_interfaces[j] == hardware_interface::HW_IF_ACCELERATION) { + msg = std_msgs::msg::Float32(); + std::get(msg).data = joint.second.command.acceleration; + } else if (&joint.second.command_interfaces[j] == hardware_interface::HW_IF_EFFORT) { + msg = std_msgs::msg::Float32(); + std::get(msg).data = joint.second.command.effort; + } else { + continue; + } + if (joint.second.command_publishers[j]) { + joint.second.command_publishers[j]->publish(msg); + } + } + } } return hardware_interface::return_type::OK; diff --git a/src/hardware/canbus/include/component_canbus_translator.hpp b/src/hardware/canbus/include/component_canbus_translator.hpp index 6f547876..3e1c1fb8 100644 --- a/src/hardware/canbus/include/component_canbus_translator.hpp +++ b/src/hardware/canbus/include/component_canbus_translator.hpp @@ -55,6 +55,7 @@ class CANTranslator : public rclcpp::Node, public CanInterface { // ADD PUBS FOR CAN TOPICS HERE rclcpp::Publisher::SharedPtr steering_angle_pub_; rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr av_position_pub_; rclcpp::Publisher::SharedPtr wss_velocity_pub1_; rclcpp::Publisher::SharedPtr wss_velocity_pub2_; rclcpp::Publisher::SharedPtr wss_velocity_pub3_; diff --git a/src/hardware/canbus/src/component_canbus_translator.cpp b/src/hardware/canbus/src/component_canbus_translator.cpp index e1003114..ff5ea3c5 100644 --- a/src/hardware/canbus/src/component_canbus_translator.cpp +++ b/src/hardware/canbus/src/component_canbus_translator.cpp @@ -86,6 +86,18 @@ void CANTranslator::canmsg_timer() { std_msgs::msg::Float64::UniquePtr position_msg(new std_msgs::msg::Float64()); position_msg->data = wheel_positions_[vesc_id]; wheel_position_pubs_[vesc_id]->publish(std::move(position_msg)); + + if (vesc_id > 2) { + double av_position = 0; + for (int i = 2; i < 4; i++) { + av_position += wheel_positions_[i]; + } + av_position = av_position / 2; + + // Publish msg with virtual center wheel position + std_msgs::msg::Float64::UniquePtr virtual_rear_wheel_position(new std_msgs::msg::Float64()); + virtual_rear_wheel_position->data = av_position; + } } } // Steering Angle @@ -192,6 +204,8 @@ CANTranslator::CANTranslator(const rclcpp::NodeOptions &options) : Node("canbus_ wheel_position_pubs_.push_back(wss_position_pub2_); wheel_position_pubs_.push_back(wss_position_pub3_); wheel_position_pubs_.push_back(wss_position_pub4_); + // Average rear wheel position + av_position_pub_ = this->create_publisher("/vehicle/rw_position", 10); // Wheel velocity wss_velocity_pub1_ = this->create_publisher("/vehicle/wheel_speed1", 10);