Added stamped param

Signed-off-by: Jakub Delicat <[email protected]>

joy2twist/config/joy2twist.yaml

@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    cmd_vel.use_stamped: false
+
     linear_velocity_factor:
       fast: 1.0
       regular: 0.5

joy2twist/config/joy2twist_panther.yaml

@@ -1,5 +1,7 @@
 /**:
   ros__parameters:
+    cmd_vel.use_stamped: false
+
     linear_velocity_factor:
       fast: 2.0
       regular: 1.2

joy2twist/include/joy2twist/joy2twist_node.hpp

@@ -8,6 +8,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <sensor_msgs/msg/joy.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
@@ -16,6 +17,7 @@ namespace joy2twist
 {
 using MsgJoy = sensor_msgs::msg::Joy;
 using MsgTwist = geometry_msgs::msg::Twist;
+using MsgTwistStamped = geometry_msgs::msg::TwistStamped;
 using MsgBool = std_msgs::msg::Bool;
 using SrvTrigger = std_srvs::srv::Trigger;
 
@@ -51,11 +53,13 @@ class Joy2TwistNode : public rclcpp::Node
   void trigger_service_cb(
     const rclcpp::Client<SrvTrigger>::SharedFuture & future,
     const std::string & service_name) const;
+  void publish_twist(const MsgTwist & twist_msg);
 
   std::map<std::string, float> linear_velocity_factors_;
   std::map<std::string, float> angular_velocity_factors_;
 
   ButtonIndex button_index_;
+  bool use_stamped_cmd_vel_;
   bool driving_mode_;
   bool e_stop_present_;
   bool e_stop_state_;
@@ -66,6 +70,7 @@ class Joy2TwistNode : public rclcpp::Node
   rclcpp::Subscription<MsgBool>::SharedPtr e_stop_sub_;
   rclcpp::Subscription<MsgJoy>::SharedPtr joy_sub_;
   rclcpp::Publisher<MsgTwist>::SharedPtr twist_pub_;
+  rclcpp::Publisher<MsgTwistStamped>::SharedPtr twist_stamped_pub_;
   rclcpp::Client<SrvTrigger>::SharedPtr e_stop_reset_client_;
   rclcpp::Client<SrvTrigger>::SharedPtr e_stop_trigger_client_;
 };

joy2twist/src/joy2twist_node.cpp

@@ -13,8 +13,14 @@ Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node")
 
   joy_sub_ = create_subscription<MsgJoy>(
     "joy", rclcpp::SensorDataQoS(), std::bind(&Joy2TwistNode::joy_cb, this, _1));
-  twist_pub_ = create_publisher<MsgTwist>(
-    "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
+
+  if(use_stamped_cmd_vel_){
+    twist_stamped_pub_ = create_publisher<MsgTwistStamped>(
+      "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
+  } else {
+    twist_pub_ = create_publisher<MsgTwist>(
+      "cmd_vel", rclcpp::QoS(rclcpp::KeepLast(1)).durability_volatile().reliable());
+  }
 
   if (e_stop_present_) {
     e_stop_sub_ = this->create_subscription<MsgBool>(
@@ -29,6 +35,8 @@ Joy2TwistNode::Joy2TwistNode() : Node("joy2twist_node")
 
 void Joy2TwistNode::declare_parameters()
 {
+  this->declare_parameter<bool>("cmd_vel.use_stamped", false);
+
   this->declare_parameter<float>("linear_velocity_factor.fast", 1.0);
   this->declare_parameter<float>("linear_velocity_factor.regular", 0.5);
   this->declare_parameter<float>("linear_velocity_factor.slow", 0.2);
@@ -54,6 +62,8 @@ void Joy2TwistNode::declare_parameters()
 
 void Joy2TwistNode::load_parameters()
 {
+  this->get_parameter<bool>("cmd_vel.use_stamped", use_stamped_cmd_vel_);
+
   this->get_parameter<float>("linear_velocity_factor.fast", linear_velocity_factors_[FAST]);
   this->get_parameter<float>("linear_velocity_factor.regular", linear_velocity_factors_[REGULAR]);
   this->get_parameter<float>("linear_velocity_factor.slow", linear_velocity_factors_[SLOW]);
@@ -73,7 +83,7 @@ void Joy2TwistNode::load_parameters()
   this->get_parameter<int>("button_index_map.fast_mode", button_index_.fast_mode);
   this->get_parameter<int>("button_index_map.slow_mode", button_index_.slow_mode);
   this->get_parameter<int>("button_index_map.e_stop_reset", button_index_.e_stop_reset);
-  this->get_parameter<int>("button_index_map.e_stop_trigger", button_index_.e_stop_trigger);
+  this->get_parameter<int>("button_index_map.e_suse_stamped_cmd_vel_top_trigger", button_index_.e_stop_trigger);
   this->get_parameter<int>(
     "button_index_map.enable_e_stop_reset", button_index_.enable_e_stop_reset);
 }
@@ -87,7 +97,7 @@ void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
   if (e_stop_present_) {
     if (joy_msg->buttons.at(button_index_.e_stop_trigger) && !e_stop_state_) {
       // Stop the robot before trying to call the e-stop trigger service
-      twist_pub_->publish(twist_msg);
+      publish_twist(twist_msg);
       call_trigger_service(e_stop_trigger_client_);
     } else if (
       joy_msg->buttons.at(button_index_.enable_e_stop_reset) &&
@@ -99,10 +109,10 @@ void Joy2TwistNode::joy_cb(const MsgJoy::SharedPtr joy_msg)
   if (joy_msg->buttons.at(button_index_.dead_man_switch)) {
     driving_mode_ = true;
     convert_joy_to_twist(joy_msg, twist_msg);
-    twist_pub_->publish(twist_msg);
+    publish_twist(twist_msg);
   } else if (driving_mode_) {
     driving_mode_ = false;
-    twist_pub_->publish(twist_msg);
+    publish_twist(twist_msg);
   }
 }
 
@@ -159,4 +169,15 @@ void Joy2TwistNode::trigger_service_cb(
   RCLCPP_INFO(this->get_logger(), "Successfully called %s service", service_name.c_str());
 }
 
+void Joy2TwistNode::publish_twist(const MsgTwist & twist_msg){
+  if(use_stamped_cmd_vel_){
+    MsgTwistStamped twist_stamped_msg;
+    twist_stamped_msg.header.stamp = this->get_clock()->now();
+    twist_stamped_msg.twist = twist_msg;
+    twist_stamped_pub_->publish(twist_stamped_msg);
+  } else {
+    twist_pub_->publish(twist_msg);
+  }
+}
+
 }  // namespace joy2twist