set ros automobile steering and throttle through a topic

cyberbotics · Mar 13, 2024 · beacd60 · beacd60
1 parent 74bfd2c
commit beacd60
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/projects/vehicles/controllers/ros_automobile/RosAutomobile.cpp b/projects/vehicles/controllers/ros_automobile/RosAutomobile.cpp
@@ -71,6 +71,9 @@ RosAutomobile::~RosAutomobile() {
  mWheelSpeedPublisher[1].shutdown();
  mWheelSpeedPublisher[2].shutdown();
  mWheelSpeedPublisher[3].shutdown();
+
+ mSteeringAngleSubscriber.shutdown();
+ mThrottleSubscriber.shutdown();
 }
 
 void RosAutomobile::setupRobot() {
@@ -151,6 +154,9 @@ void RosAutomobile::launchRos(int argc, char **argv) {
  mWheelSpeedPublisher[1] = nodeHandle()->advertise<webots_ros::Float64Stamped>("automobile/front_left_wheel_speed", 1);
  mWheelSpeedPublisher[2] = nodeHandle()->advertise<webots_ros::Float64Stamped>("automobile/rear_right_wheel_speed", 1);
  mWheelSpeedPublisher[3] = nodeHandle()->advertise<webots_ros::Float64Stamped>("automobile/rear_left_wheel_speed", 1);
+
+ mSteeringAngleSubscriber = nodeHandle()->subscribe("automobile/set_throttle", 2, &RosAutomobile::throttleCallback, this);
+ mThrottleSubscriber = nodeHandle()->subscribe("automobile/set_steering_angle", 2, &RosAutomobile::steeringAngleCallback, this);
 }
 
 void RosAutomobile::setRosDevices(const char **hiddenDevices, int numberHiddenDevices) {
@@ -322,6 +328,16 @@ bool RosAutomobile::setWiperModeCallback(webots_ros::set_int::Request &req, webo
  return true;
 }
 
+void RosAutomobile::steeringAngleCallback(const webots_ros::Float64Stamped msg)
+{
+ car()->setSteeringAngle(msg.data);
+}
+
+void RosAutomobile::throttleCallback(const webots_ros::Float64Stamped msg)
+{
+ car()->setThrottle(msg.data);
+}
+
 int RosAutomobile::step(int duration) {
  // publish topics
  webots_ros::Float64Stamped value;

diff --git a/projects/vehicles/controllers/ros_automobile/RosAutomobile.hpp b/projects/vehicles/controllers/ros_automobile/RosAutomobile.hpp
@@ -17,6 +17,7 @@
 
 #include <webots/vehicle/Car.hpp>
 
+#include <webots_ros/Float64Stamped.h>
 #include <webots_ros/get_float.h>
 #include <webots_ros/get_int.h>
 #include <webots_ros/set_bool.h>
@@ -64,6 +65,9 @@ class RosAutomobile : public Ros {
  bool setThrottleCallback(webots_ros::set_float::Request &req, webots_ros::set_float::Response &res);
  bool setWiperModeCallback(webots_ros::set_int::Request &req, webots_ros::set_int::Response &res);
 
+ void steeringAngleCallback(const webots_ros::Float64Stamped msg);
+ void throttleCallback(const webots_ros::Float64Stamped msg);
+
 protected:
  virtual void setupRobot();
  virtual void setRosDevices(const char **hiddenDevices, int numberHiddenDevices);
@@ -113,6 +117,9 @@ class RosAutomobile : public Ros {
  ros::Publisher mThrottlePublisher;
  ros::Publisher mWheelEncoderPublisher[4];
  ros::Publisher mWheelSpeedPublisher[4];
+
+ ros::Subscriber mSteeringAngleSubscriber;
+ ros::Subscriber mThrottleSubscriber;
 };
 
 #endif // ROS_AUTOMOBILE_HPP