Adapt carla_control_panel to native ROS2 interface

carla_ros_bridge/src/carla_ros_bridge/CARLA_VERSION

@@ -1 +1 @@
-0.9.13
+0.9.15

rviz_carla_plugin/src/carla_control_panel.cpp

@@ -132,15 +132,15 @@ CarlaControlPanel::CarlaControlPanel(QWidget *parent)
 
   mCarlaStatusSubscriber = mNodeHandle.subscribe("/carla/status", 1000, &CarlaControlPanel::carlaStatusChanged, this);
   mCarlaControlPublisher = mNodeHandle.advertise<carla_msgs::CarlaControl>("/carla/control", 10);
-  mEgoVehicleStatusSubscriber = mNodeHandle.subscribe(
-    "/carla/ego_vehicle/vehicle_status", 1000, &CarlaControlPanel::egoVehicleStatusChanged, this);
-  mEgoVehicleOdometrySubscriber
-    = mNodeHandle.subscribe("/carla/ego_vehicle/odometry", 1000, &CarlaControlPanel::egoVehicleOdometryChanged, this);
+  mVehicleStatusSubscriber = mNodeHandle.subscribe(
+    "/carla/ego_vehicle/vehicle_status", 1000, &CarlaControlPanel::vehicleStatusChanged, this);
+  mVehicleOdometrySubscriber
+    = mNodeHandle.subscribe("/carla/ego_vehicle/odometry", 1000, &CarlaControlPanel::vehicleOdometryChanged, this);
 
   mCameraPosePublisher
     = mNodeHandle.advertise<geometry_msgs::Pose>("carla/ego_vehicle/spectator_pose", 10, true);
 
-  mEgoVehicleControlManualOverridePublisher
+  mVehicleControlManualOverridePublisher
     = mNodeHandle.advertise<std_msgs::Bool>("/carla/ego_vehicle/vehicle_control_manual_override", 1, true);
 
   mExecuteScenarioClient
@@ -243,7 +243,7 @@ void CarlaControlPanel::overrideVehicleControl(int state)
     boolMsg.data = false;
     mDriveWidget->setEnabled(false);
   }
-  mEgoVehicleControlManualOverridePublisher.publish(boolMsg);
+  mVehicleControlManualOverridePublisher.publish(boolMsg);
 }
 
 void CarlaControlPanel::scenarioRunnerStatusChanged(
@@ -297,7 +297,7 @@ void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_ty
   setScenarioRunnerStatus(mScenarioSelection->count() > 0);
 }
 
-void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg)
+void CarlaControlPanel::vehicleStatusChanged(const carla_msgs::CarlaVehicleStatus::ConstPtr &msg)
 {
   mOverrideVehicleControl->setEnabled(true);
   mSteerBar->setValue(msg->control.steer * 100);
@@ -309,7 +309,7 @@ void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicl
   mSpeedLabel->setText(speedStream.str().c_str());
 }
 
-void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg)
+void CarlaControlPanel::vehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg)
 {
   std::stringstream posStream;
   posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y;

rviz_carla_plugin/src/carla_control_panel.h

@@ -7,7 +7,7 @@
 #pragma once
 
 #include <OgreCamera.h>
-#include <carla_msgs/CarlaEgoVehicleStatus.h>
+#include <carla_msgs/CarlaVehicleStatus.h>
 #include <carla_msgs/CarlaStatus.h>
 #include <carla_ros_scenario_runner_types/CarlaScenarioList.h>
 #include <carla_ros_scenario_runner_types/CarlaScenarioRunnerStatus.h>
@@ -60,8 +60,8 @@ protected Q_SLOTS:
 
   void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::CarlaScenarioRunnerStatus::ConstPtr &msg);
   void carlaStatusChanged(const carla_msgs::CarlaStatus::ConstPtr &msg);
-  void egoVehicleStatusChanged(const carla_msgs::CarlaEgoVehicleStatus::ConstPtr &msg);
-  void egoVehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg);
+  void vehicleStatusChanged(const carla_msgs::CarlaVehicleStatus::ConstPtr &msg);
+  void vehicleOdometryChanged(const nav_msgs::Odometry::ConstPtr &msg);
   void carlaScenariosChanged(const carla_ros_scenario_runner_types::CarlaScenarioList::ConstPtr &msg);
   carla_msgs::CarlaStatus::ConstPtr mCarlaStatus{nullptr};
 
@@ -80,10 +80,10 @@ protected Q_SLOTS:
   IndicatorWidget *mIndicatorWidget;
   ros::Publisher mTwistPublisher;
   ros::Publisher mCarlaControlPublisher;
-  ros::Publisher mEgoVehicleControlManualOverridePublisher;
+  ros::Publisher mVehicleControlManualOverridePublisher;
   ros::Subscriber mCarlaStatusSubscriber;
-  ros::Subscriber mEgoVehicleStatusSubscriber;
-  ros::Subscriber mEgoVehicleOdometrySubscriber;
+  ros::Subscriber mVehicleStatusSubscriber;
+  ros::Subscriber mVehicleOdometrySubscriber;
   ros::ServiceClient mExecuteScenarioClient;
   ros::Subscriber mScenarioSubscriber;
   ros::Subscriber mScenarioRunnerStatusSubscriber;

rviz_carla_plugin/src/carla_control_panel_ROS2.cpp

@@ -175,29 +175,29 @@ void CarlaControlPanel::onInitialize()
   _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 
   // set up ros subscriber and publishers
-  mCarlaStatusSubscriber = _node->create_subscription<carla_msgs::msg::CarlaStatus>("/carla/status", 1000, std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1));
-  mCarlaControlPublisher = _node->create_publisher<carla_msgs::msg::CarlaControl>("/carla/control", 10);
-  mEgoVehicleStatusSubscriber 
-    = _node->create_subscription<carla_msgs::msg::CarlaEgoVehicleStatus>("/carla/ego_vehicle/vehicle_status", 1000, std::bind(&CarlaControlPanel::egoVehicleStatusChanged, this, _1));
-  mEgoVehicleOdometrySubscriber
-    = _node->create_subscription<nav_msgs::msg::Odometry>("/carla/ego_vehicle/odometry", 1000, std::bind(&CarlaControlPanel::egoVehicleOdometryChanged, this, _1));
+  mCarlaStatusSubscriber = _node->create_subscription<carla_msgs::msg::CarlaStatus>("/carla/world/status", rclcpp::SensorDataQoS().keep_last(10), std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1));
+  mCarlaControlPublisher = _node->create_publisher<carla_msgs::msg::CarlaControl>("/carla/world/control", 10);
+  mVehicleStatusSubscriber 
+    = _node->create_subscription<carla_msgs::msg::CarlaVehicleStatus>("/carla/vehicles/hero/vehicle_status", 1000, std::bind(&CarlaControlPanel::vehicleStatusChanged, this, _1));
+  mVehicleOdometrySubscriber
+    = _node->create_subscription<nav_msgs::msg::Odometry>("/carla/vehicles/hero/odometry", 1000, std::bind(&CarlaControlPanel::vehicleOdometryChanged, this, _1));
 
   auto qos_latch_10 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 10));
   qos_latch_10.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
   mCameraPosePublisher
-    = _node->create_publisher<geometry_msgs::msg::Pose>("/carla/ego_vehicle/spectator_pose", qos_latch_10);
+    = _node->create_publisher<geometry_msgs::msg::Pose>("/carla/vehicles/hero/spectator_pose", qos_latch_10);
 
   auto qos_latch_1 = rclcpp::QoS( rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1));
   qos_latch_1.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
-  mEgoVehicleControlManualOverridePublisher
-    = _node->create_publisher<std_msgs::msg::Bool>("/carla/ego_vehicle/vehicle_control_manual_override", qos_latch_1);
+  mVehicleControlManualOverridePublisher
+    = _node->create_publisher<std_msgs::msg::Bool>("/carla/vehicles/hero/vehicle_control_manual_override", qos_latch_1);
 
   mExecuteScenarioClient
     = _node->create_client<carla_ros_scenario_runner_types::srv::ExecuteScenario>("/scenario_runner/execute_scenario");
   mScenarioRunnerStatusSubscriber
     = _node->create_subscription<carla_ros_scenario_runner_types::msg::CarlaScenarioRunnerStatus>("/scenario_runner/status", 10, std::bind(&CarlaControlPanel::scenarioRunnerStatusChanged, this, _1));
 
-  mTwistPublisher = _node->create_publisher<geometry_msgs::msg::Twist>("/carla/ego_vehicle/twist", 1);
+  mTwistPublisher = _node->create_publisher<geometry_msgs::msg::Twist>("/carla/vehicles/hero/twist", 1);
 
   mScenarioSubscriber
     = _node->create_subscription<carla_ros_scenario_runner_types::msg::CarlaScenarioList>("/carla/available_scenarios", 1, std::bind(&CarlaControlPanel::carlaScenariosChanged, this, _1));
@@ -249,7 +249,7 @@ void CarlaControlPanel::overrideVehicleControl(int state)
     boolMsg.data = false;
     mDriveWidget->setEnabled(false);
   }
-  mEgoVehicleControlManualOverridePublisher->publish(boolMsg);
+  mVehicleControlManualOverridePublisher->publish(boolMsg);
 }
 
 void CarlaControlPanel::scenarioRunnerStatusChanged(
@@ -303,19 +303,19 @@ void CarlaControlPanel::carlaScenariosChanged(const carla_ros_scenario_runner_ty
   setScenarioRunnerStatus(mScenarioSelection->count() > 0);
 }
 
-void CarlaControlPanel::egoVehicleStatusChanged(const carla_msgs::msg::CarlaEgoVehicleStatus::SharedPtr msg)
+void CarlaControlPanel::vehicleStatusChanged(const carla_msgs::msg::CarlaVehicleStatus::SharedPtr msg)
 {
   mOverrideVehicleControl->setEnabled(true);
-  mSteerBar->setValue(msg->control.steer * 100);
-  mThrottleBar->setValue(msg->control.throttle * 100);
-  mBrakeBar->setValue(msg->control.brake * 100);
+  mSteerBar->setValue(msg->last_applied_vehicle_control.steer * 100);
+  mThrottleBar->setValue(msg->last_applied_vehicle_control.throttle * 100);
+  mBrakeBar->setValue(msg->last_applied_vehicle_control.brake * 100);
 
   std::stringstream speedStream;
   speedStream << std::fixed << std::setprecision(2) << msg->velocity * 3.6;
   mSpeedLabel->setText(speedStream.str().c_str());
 }
 
-void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg)
+void CarlaControlPanel::vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg)
 {
   std::stringstream posStream;
   posStream << std::fixed << std::setprecision(2) << msg->pose.pose.position.x << ", " << msg->pose.pose.position.y;
@@ -328,7 +328,7 @@ void CarlaControlPanel::egoVehicleOdometryChanged(const nav_msgs::msg::Odometry:
 
 void CarlaControlPanel::setSimulationButtonStatus(bool active)
 {
-  if (active)
+  if (true)
   {
     mPlayPauseButton->setDisabled(false);
     mPlayPauseButton->setToolTip("Play/Pause the CARLA world.");

rviz_carla_plugin/src/carla_control_panel_ROS2.h

@@ -7,7 +7,7 @@
 #pragma once
 
 #include <OgreCamera.h>
-#include <carla_msgs/msg/carla_ego_vehicle_status.hpp>
+#include <carla_msgs/msg/carla_vehicle_status.hpp>
 #include <carla_msgs/msg/carla_status.hpp>
 #include <carla_msgs/msg/carla_control.hpp>
 #include <carla_ros_scenario_runner_types/msg/carla_scenario_list.hpp>
@@ -67,8 +67,8 @@ protected Q_SLOTS:
 
   void scenarioRunnerStatusChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioRunnerStatus::SharedPtr msg);
   void carlaStatusChanged(const carla_msgs::msg::CarlaStatus::SharedPtr msg);
-  void egoVehicleStatusChanged(const carla_msgs::msg::CarlaEgoVehicleStatus::SharedPtr msg);
-  void egoVehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void vehicleStatusChanged(const carla_msgs::msg::CarlaVehicleStatus::SharedPtr msg);
+  void vehicleOdometryChanged(const nav_msgs::msg::Odometry::SharedPtr msg);
   void carlaScenariosChanged(const carla_ros_scenario_runner_types::msg::CarlaScenarioList::SharedPtr msg);
   carla_msgs::msg::CarlaStatus::SharedPtr mCarlaStatus{nullptr};
 
@@ -89,10 +89,10 @@ protected Q_SLOTS:
   IndicatorWidget *mIndicatorWidget;
   rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr mTwistPublisher;
   rclcpp::Publisher<carla_msgs::msg::CarlaControl>::SharedPtr mCarlaControlPublisher;
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr mEgoVehicleControlManualOverridePublisher;
+  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr mVehicleControlManualOverridePublisher;
   rclcpp::Subscription<carla_msgs::msg::CarlaStatus>::SharedPtr mCarlaStatusSubscriber;
-  rclcpp::Subscription<carla_msgs::msg::CarlaEgoVehicleStatus>::SharedPtr mEgoVehicleStatusSubscriber;
-  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mEgoVehicleOdometrySubscriber;
+  rclcpp::Subscription<carla_msgs::msg::CarlaVehicleStatus>::SharedPtr mVehicleStatusSubscriber;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mVehicleOdometrySubscriber;
   rclcpp::Client<carla_ros_scenario_runner_types::srv::ExecuteScenario>::SharedPtr mExecuteScenarioClient;
   rclcpp::Subscription<carla_ros_scenario_runner_types::msg::CarlaScenarioList>::SharedPtr mScenarioSubscriber;
   rclcpp::Subscription<carla_ros_scenario_runner_types::msg::CarlaScenarioRunnerStatus>::SharedPtr mScenarioRunnerStatusSubscriber;