diff --git a/carla_msgs b/carla_msgs index 2ec9a238..e13cc4b3 160000 --- a/carla_msgs +++ b/carla_msgs @@ -1 +1 @@ -Subproject commit 2ec9a2387ed6f18d236c7252b76c8f2e567611ae +Subproject commit e13cc4b38088f27987bb2bf9ea57c4da19bc5de2 diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp index 00366d26..84b70c01 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.cpp @@ -175,12 +175,10 @@ void CarlaControlPanel::onInitialize() _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); // set up ros subscriber and publishers - mCarlaStatusSubscriber = _node->create_subscription("/carla/world/status", rclcpp::SensorDataQoS().keep_last(10), std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1)); + mCarlaStatusSubscriber = _node->create_subscription("/carla/world/status", rclcpp::SensorDataQoS().keep_last(10).transient_local(), std::bind(&CarlaControlPanel::carlaStatusChanged, this, _1)); mCarlaControlPublisher = _node->create_publisher("/carla/world/control", 10); mVehicleStatusSubscriber = _node->create_subscription("/carla/vehicles/hero/vehicle_status", 1000, std::bind(&CarlaControlPanel::vehicleStatusChanged, this, _1)); - mVehicleOdometrySubscriber - = _node->create_subscription("/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); @@ -313,16 +311,13 @@ void CarlaControlPanel::vehicleStatusChanged(const carla_msgs::msg::CarlaVehicle std::stringstream speedStream; speedStream << std::fixed << std::setprecision(2) << msg->velocity * 3.6; mSpeedLabel->setText(speedStream.str().c_str()); -} -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; + posStream << std::fixed << std::setprecision(2) << msg->pose.position.x << ", " << msg->pose.position.y; mPosLabel->setText(posStream.str().c_str()); std::stringstream headingStream; - headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(msg->pose.pose.orientation); + headingStream << std::fixed << std::setprecision(2) << tf2::getYaw(msg->pose.orientation); mHeadingLabel->setText(headingStream.str().c_str()); } @@ -347,11 +342,11 @@ void CarlaControlPanel::setSimulationButtonStatus(bool active) void CarlaControlPanel::carlaStatusChanged(const carla_msgs::msg::CarlaStatus::SharedPtr msg) { mCarlaStatus = msg; - setSimulationButtonStatus(mCarlaStatus->synchronous_mode); + setSimulationButtonStatus(mCarlaStatus->episode_settings.synchronous_mode); - if (mCarlaStatus->synchronous_mode) + if (mCarlaStatus->episode_settings.synchronous_mode) { - if (mCarlaStatus->synchronous_mode_running) + if (mCarlaStatus->game_running) { QPixmap pixmap(":/icons/pause.png"); QIcon iconPause(pixmap); @@ -378,7 +373,7 @@ void CarlaControlPanel::carlaTogglePlayPause() if (mCarlaStatus) { carla_msgs::msg::CarlaControl ctrl; - if (mCarlaStatus->synchronous_mode_running) + if (mCarlaStatus->game_running) { ctrl.command = carla_msgs::msg::CarlaControl::PAUSE; } diff --git a/rviz_carla_plugin/src/carla_control_panel_ROS2.h b/rviz_carla_plugin/src/carla_control_panel_ROS2.h index 93eec74f..46a99834 100644 --- a/rviz_carla_plugin/src/carla_control_panel_ROS2.h +++ b/rviz_carla_plugin/src/carla_control_panel_ROS2.h @@ -12,7 +12,6 @@ #include #include #include -#include #include "rclcpp/rclcpp.hpp" #include #include @@ -68,7 +67,6 @@ 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 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}; @@ -92,7 +90,6 @@ protected Q_SLOTS: rclcpp::Publisher::SharedPtr mVehicleControlManualOverridePublisher; rclcpp::Subscription::SharedPtr mCarlaStatusSubscriber; rclcpp::Subscription::SharedPtr mVehicleStatusSubscriber; - rclcpp::Subscription::SharedPtr mVehicleOdometrySubscriber; rclcpp::Client::SharedPtr mExecuteScenarioClient; rclcpp::Subscription::SharedPtr mScenarioSubscriber; rclcpp::Subscription::SharedPtr mScenarioRunnerStatusSubscriber;