Skip to content

Commit

Permalink
Changed map<srting,vector<vector<char>>> to structs and variants when…
Browse files Browse the repository at this point in the history 
… storing topic and interface info
  • Loading branch information
@leungw16
leungw16 committed Sep 22, 2024
1 parent 0694350 commit 06d4fe7
Show file tree
Hide file tree
Showing 5 changed files with 295 additions and 77 deletions.
21 changes: 21 additions & 0 deletions src/common/vehicle_urdf/urdf/ros2_control.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
<hardware>
<plugin>mock_components/GenericSystem</plugin>
<param name="calculate_dynamics">true</param>
<param name="ackermann_pub_topic">/vehicle/ackermann_cmd</param>
</hardware>
<joint name="rear_left_wheel_joint">
<command_interface name="acceleration">
Expand All @@ -14,6 +15,7 @@
<state_interface name="velocity">
<param name="initial_value">0</param>
</state_interface>
<param name="joint_type">driving</param>
</joint>
<joint name="rear_right_wheel_joint">
<command_interface name="acceleration">
Expand All @@ -24,13 +26,32 @@
<state_interface name="velocity">
<param name="initial_value">0</param>
</state_interface>
<param name="joint_type">driving</param>
</joint>
<joint name="rear_virtual_wheel_joint">
<command_interface name="acceleration">
</command_interface>
<state_interface name="position">
<param name="initial_value">0</param>
<!-- Custom params not implemented in ROS Humble. Co-opting unused param instead -->
<param name="data_type">/vehicle/rw_position</param>
</state_interface>
<state_interface name="velocity">
<param name="initial_value">0</param>
<!-- Custom params not implemented in ROS Humble. Co-opting unused param instead -->
<param name="data_type">/vehicle/velocity</param>
</state_interface>
<param name="joint_type">driving</param>
</joint>
<joint name="virtual_steering_hinge_joint">
<command_interface name="position">
</command_interface>
<state_interface name="position">
<param name="initial_value">0</param>
<!-- Custom params not implemented in ROS Humble. Co-opting unused param instead -->
<param name="data_type">/vehicle/steering_angle</param>
</state_interface>
<param name="joint_type">steering</param>
</joint>
</ros2_control>
</robot>
104 changes: 80 additions & 24 deletions ...l/ros2_control/qutms_hw_interfaces/include/qutms_hw_interfaces/qev_3d_topic_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,9 @@
#define QUTMS_HW_INTERFACES__QEV_3D_TOPIC_INTERFACE_HPP_

#include <memory>
#include <set>
#include <string>
#include <variant>
#include <vector>

#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
Expand All @@ -36,26 +38,92 @@

namespace qutms_hw_interfaces {
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
using MessageTypeVariant = std::variant<ackermann_msgs::msg::AckermannDriveStamped, driverless_msgs::msg::State,
std_msgs::msg::Float32, std_msgs::msg::Float64>;

using PublisherVariant = std::variant<rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr,
rclcpp::Publisher<driverless_msgs::msg::State>::SharedPtr,
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr,
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr>;

using SubscriberVariant = std::variant<rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr,
rclcpp::Subscription<driverless_msgs::msg::State>::SharedPtr,
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr,
rclcpp::Subscription<std_msgs::msg::Float64>::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();
}

Joint() = default;

std::string joint_name;
std::string joint_type;
std::vector<char> command_interfaces;
// Currently unused: For future use
std::vector<std::optional<PublisherWrapper>> command_publishers;
std::vector<char> state_interfaces;
std::vector<std::optional<SubscriberWrapper>> state_subscribers;
JointValue state;
JointValue command;
};

class PublisherWrapper {
public:
template <typename T>
PublisherWrapper(typename rclcpp::Publisher<T>::SharedPtr publisher) : publisher_(publisher) {}

void publish(const MessageTypeVariant& msgVariant) {
std::visit(
[this](auto& msg) {
using MsgType = std::decay_t<decltype(msg)>;
if (auto pub =
std::get_if<typename rclcpp::Publisher<typename std::decay<decltype(msg)>::type>::SharedPtr>(
&publisher_)) {
(*pub)->publish(msg);
}
},
msgVariant);
}

private:
std::variant<rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr,
rclcpp::Publisher<driverless_msgs::msg::State>::SharedPtr,
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr,
rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr>
publisher_;
};

class SubscriberWrapper {
public:
template <typename T>
SubscriberWrapper(typename rclcpp::Subscription<T>::SharedPtr subscriber) : subscriber_(std::move(subscriber)) {}

// MessageTypeVariant handle_message(const MessageTypeVariant& msgVariant){
// std::visit([this](auto& msg){
// using MsgType = std::decay_t<decltype(msg)>;
// if (auto sub = std::get_if<rclcpp::Subscription<MsgType>::SharedPtr>(&subscriber_)){
// return msg;
// }
// }, msgVariant);
// }
private:
std::variant<rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr,
rclcpp::Subscription<driverless_msgs::msg::State>::SharedPtr,
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr,
rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr>
subscriber_;
};

class Qev3dTopicInterface : public hardware_interface::SystemInterface {
public:
TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
Expand Down Expand Up @@ -85,6 +153,7 @@ class Qev3dTopicInterface : public hardware_interface::SystemInterface {
private:
std::vector<double> hw_commands_;
std::vector<double> hw_states_;

// Definition for joints that are configured for this interface
// Format:
// {
Expand All @@ -104,12 +173,13 @@ class Qev3dTopicInterface : public hardware_interface::SystemInterface {
// },
// {Joint_type(1 Unique)->{steering/drive/other}}
// }),
const std::map<std::string, std::vector<std::vector<std::string>>> 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<std::string, std::vector<std::vector<char>>> 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<std::string> available_joints = {"virtual_steering_hinge_joint", "virtual_rear_wheel_joint"};
std::map<std::string, Joint> hw_interfaces_;
rclcpp::Subscription<driverless_msgs::msg::State>::SharedPtr status_sub_;
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr velocity_sub_;
Expand All @@ -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
Expand Down
Loading

0 comments on commit 06d4fe7

Please sign in to comment.