diff --git a/projects/vehicles/controllers/ros_automobile/RosAutomobile.cpp b/projects/vehicles/controllers/ros_automobile/RosAutomobile.cpp index e2ddb376e97..63a6c3cbd50 100644 --- 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("automobile/front_left_wheel_speed", 1); mWheelSpeedPublisher[2] = nodeHandle()->advertise("automobile/rear_right_wheel_speed", 1); mWheelSpeedPublisher[3] = nodeHandle()->advertise("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 index 0580ceb5ea1..f4fc9446400 100644 --- a/projects/vehicles/controllers/ros_automobile/RosAutomobile.hpp +++ b/projects/vehicles/controllers/ros_automobile/RosAutomobile.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -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