Skip to content

Commit

Permalink
feat!: replace autoware_auto_msgs with autoware_msgs for simulator mo…
Browse files Browse the repository at this point in the history
…dules

Signed-off-by: Ryohsuke Mitsudome <[email protected]>
Co-authored-by: Cynthia Liu <[email protected]>
Co-authored-by: NorahXiong <[email protected]>
Co-authored-by: beginningfan <[email protected]>
  • Loading branch information
4 people authored and xmfcx committed Jun 4, 2024
1 parent 2332541 commit ead0068
Show file tree
Hide file tree
Showing 19 changed files with 106 additions and 124 deletions.
4 changes: 2 additions & 2 deletions simulator/dummy_perception_publisher/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -7,14 +7,14 @@ autoware_package()
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/InitialState.msg"
"msg/Object.msg"
DEPENDENCIES autoware_auto_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs
DEPENDENCIES autoware_perception_msgs tier4_perception_msgs geometry_msgs std_msgs unique_identifier_msgs
)

# See ndt_omp package for documentation on why PCL is special
find_package(PCL REQUIRED COMPONENTS common filters)

set(${PROJECT_NAME}_DEPENDENCIES
autoware_auto_perception_msgs
autoware_perception_msgs
tier4_perception_msgs
pcl_conversions
rclcpp
Expand Down
2 changes: 1 addition & 1 deletion simulator/dummy_perception_publisher/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ This node publishes the result of the dummy detection with the type of perceptio
| ----------------------------------- | -------------------------------------------------------- | ----------------------- |
| `output/dynamic_object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | dummy detection objects |
| `output/points_raw` | `sensor_msgs::msg::PointCloud2` | point cloud of objects |
| `output/debug/ground_truth_objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | ground truth objects |
| `output/debug/ground_truth_objects` | `autoware_perception_msgs::msg::TrackedObjects` | ground truth objects |

## Parameters

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
#include <autoware_perception_msgs/msg/detected_objects.hpp>
#include <autoware_perception_msgs/msg/tracked_objects.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>

Expand Down Expand Up @@ -59,7 +59,7 @@ struct ObjectInfo
geometry_msgs::msg::PoseWithCovariance pose_covariance_;
// convert to TrackedObject
// (todo) currently need object input to get id and header information, but it should be removed
autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(
autoware_perception_msgs::msg::TrackedObject toTrackedObject(
const dummy_perception_publisher::msg::Object & object) const;
};

Expand Down Expand Up @@ -114,7 +114,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
rclcpp::Publisher<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
detected_object_with_feature_pub_;
rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
rclcpp::Publisher<autoware_perception_msgs::msg::TrackedObjects>::SharedPtr
ground_truth_objects_pub_;
rclcpp::Subscription<dummy_perception_publisher::msg::Object>::SharedPtr object_sub_;
rclcpp::TimerBase::SharedPtr timer_;
Expand Down
4 changes: 2 additions & 2 deletions simulator/dummy_perception_publisher/msg/Object.msg
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
std_msgs/Header header
unique_identifier_msgs/UUID id
dummy_perception_publisher/InitialState initial_state
autoware_auto_perception_msgs/ObjectClassification classification
autoware_auto_perception_msgs/Shape shape
autoware_perception_msgs/ObjectClassification classification
autoware_perception_msgs/Shape shape
float32 max_velocity
float32 min_velocity

Expand Down
2 changes: 1 addition & 1 deletion simulator/dummy_perception_publisher/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_perception_msgs</depend>
<depend>geometry_msgs</depend>
<depend>libpcl-all-dev</depend>
<depend>pcl_conversions</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
#include <autoware_perception_msgs/msg/predicted_objects.hpp>

#include <memory>
#include <utility>
Expand All @@ -24,23 +24,21 @@ class EmptyObjectsPublisher : public rclcpp::Node
public:
EmptyObjectsPublisher() : Node("empty_objects_publisher")
{
empty_objects_pub_ =
this->create_publisher<autoware_auto_perception_msgs::msg::PredictedObjects>(
"~/output/objects", 1);
empty_objects_pub_ = this->create_publisher<autoware_perception_msgs::msg::PredictedObjects>(
"~/output/objects", 1);

using std::chrono_literals::operator""ms;
timer_ = rclcpp::create_timer(
this, get_clock(), 100ms, std::bind(&EmptyObjectsPublisher::timerCallback, this));
}

private:
rclcpp::Publisher<autoware_auto_perception_msgs::msg::PredictedObjects>::SharedPtr
empty_objects_pub_;
rclcpp::Publisher<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr empty_objects_pub_;
rclcpp::TimerBase::SharedPtr timer_;

void timerCallback()
{
autoware_auto_perception_msgs::msg::PredictedObjects empty_objects;
autoware_perception_msgs::msg::PredictedObjects empty_objects;
empty_objects.header.frame_id = "map";
empty_objects.header.stamp = this->now();
empty_objects_pub_->publish(empty_objects);
Expand Down
10 changes: 5 additions & 5 deletions simulator/dummy_perception_publisher/src/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@
#include <utility>
#include <vector>

using autoware_auto_perception_msgs::msg::TrackedObject;
using autoware_auto_perception_msgs::msg::TrackedObjects;
using autoware_perception_msgs::msg::TrackedObject;
using autoware_perception_msgs::msg::TrackedObjects;

ObjectInfo::ObjectInfo(
const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time)
Expand Down Expand Up @@ -169,7 +169,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode()
// optional ground truth publisher
if (publish_ground_truth_objects_) {
ground_truth_objects_pub_ =
this->create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>(
this->create_publisher<autoware_perception_msgs::msg::TrackedObjects>(
"~/output/debug/ground_truth_objects", qos);
}

Expand All @@ -182,7 +182,7 @@ void DummyPerceptionPublisherNode::timerCallback()
{
// output msgs
tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg;
autoware_auto_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg;
autoware_perception_msgs::msg::TrackedObjects output_ground_truth_objects_msg;
geometry_msgs::msg::PoseStamped output_moved_object_pose;
sensor_msgs::msg::PointCloud2 output_pointcloud_msg;
std_msgs::msg::Header header;
Expand Down Expand Up @@ -282,7 +282,7 @@ void DummyPerceptionPublisherNode::timerCallback()
feature_object.object.kinematics.twist_with_covariance =
object.initial_state.twist_covariance;
feature_object.object.kinematics.orientation_availability =
autoware_auto_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE;
feature_object.object.kinematics.has_twist = false;
tf2::toMsg(
tf_base_link2noised_moved_object,
Expand Down
22 changes: 11 additions & 11 deletions simulator/simple_planning_simulator/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -18,23 +18,23 @@ The purpose of this simulator is for the integration test of planning and contro
### input

- input/initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose
- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle
- input/manual_ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : manual target command to drive a vehicle (used when control_mode_request = Manual)
- input/gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command.
- input/manual_gear_command [`autoware_auto_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual)
- input/turn_indicators_command [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command
- input/hazard_lights_command [`autoware_auto_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command
- input/ackermann_control_command [`autoware_control_msgs/msg/Control`] : target command to drive a vehicle
- input/manual_ackermann_control_command [`autoware_control_msgs/msg/Control`] : manual target command to drive a vehicle (used when control_mode_request = Manual)
- input/gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command.
- input/manual_gear_command [`autoware_vehicle_msgs/msg/GearCommand`] : target gear command (used when control_mode_request = Manual)
- input/turn_indicators_command [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`] : target turn indicator command
- input/hazard_lights_command [`autoware_vehicle_msgs/msg/HazardLightsCommand`] : target hazard lights command
- input/control_mode_request [`tier4_vehicle_msgs::srv::ControlModeRequest`] : mode change for Auto/Manual driving

### output

- /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link)
- /output/odometry [`nav_msgs/msg/Odometry`] : simulated vehicle pose and twist
- /output/steering [`autoware_auto_vehicle_msgs/msg/SteeringReport`] : simulated steering angle
- /output/control_mode_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual)
- /output/gear_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated gear
- /output/turn_indicators_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status
- /output/hazard_lights_report [`autoware_auto_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status
- /output/steering [`autoware_vehicle_msgs/msg/SteeringReport`] : simulated steering angle
- /output/control_mode_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : current control mode (Auto/Manual)
- /output/gear_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated gear
- /output/turn_indicators_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated turn indicator status
- /output/hazard_lights_report [`autoware_vehicle_msgs/msg/ControlModeReport`] : simulated hazard lights status

## Inner-workings / Algorithms

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,23 +20,20 @@
#include "simple_planning_simulator/visibility_control.hpp"
#include "tier4_api_utils/tier4_api_utils.hpp"

#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
#include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp"
#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp"
#include "autoware_control_msgs/msg/control.hpp"
#include "autoware_map_msgs/msg/lanelet_map_bin.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "autoware_vehicle_msgs/msg/control_mode_report.hpp"
#include "autoware_vehicle_msgs/msg/engage.hpp"
#include "autoware_vehicle_msgs/msg/gear_command.hpp"
#include "autoware_vehicle_msgs/msg/gear_report.hpp"
#include "autoware_vehicle_msgs/msg/hazard_lights_command.hpp"
#include "autoware_vehicle_msgs/msg/hazard_lights_report.hpp"
#include "autoware_vehicle_msgs/msg/steering_report.hpp"
#include "autoware_vehicle_msgs/msg/turn_indicators_command.hpp"
#include "autoware_vehicle_msgs/msg/turn_indicators_report.hpp"
#include "autoware_vehicle_msgs/msg/velocity_report.hpp"
#include "autoware_vehicle_msgs/srv/control_mode_command.hpp"
#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
#include "geometry_msgs/msg/pose.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
Expand All @@ -62,22 +59,20 @@ namespace simulation
namespace simple_planning_simulator
{

using autoware_auto_control_msgs::msg::AckermannControlCommand;
using autoware_auto_geometry_msgs::msg::Complex32;
using autoware_auto_mapping_msgs::msg::HADMapBin;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::ControlModeReport;
using autoware_auto_vehicle_msgs::msg::Engage;
using autoware_auto_vehicle_msgs::msg::GearCommand;
using autoware_auto_vehicle_msgs::msg::GearReport;
using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
using autoware_auto_vehicle_msgs::msg::HazardLightsReport;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
using autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
using autoware_auto_vehicle_msgs::msg::VelocityReport;
using autoware_auto_vehicle_msgs::srv::ControlModeCommand;
using autoware_control_msgs::msg::Control;
using autoware_map_msgs::msg::LaneletMapBin;
using autoware_planning_msgs::msg::Trajectory;
using autoware_vehicle_msgs::msg::ControlModeReport;
using autoware_vehicle_msgs::msg::Engage;
using autoware_vehicle_msgs::msg::GearCommand;
using autoware_vehicle_msgs::msg::GearReport;
using autoware_vehicle_msgs::msg::HazardLightsCommand;
using autoware_vehicle_msgs::msg::HazardLightsReport;
using autoware_vehicle_msgs::msg::SteeringReport;
using autoware_vehicle_msgs::msg::TurnIndicatorsCommand;
using autoware_vehicle_msgs::msg::TurnIndicatorsReport;
using autoware_vehicle_msgs::msg::VelocityReport;
using autoware_vehicle_msgs::srv::ControlModeCommand;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
Expand Down Expand Up @@ -143,10 +138,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
rclcpp::Subscription<TurnIndicatorsCommand>::SharedPtr sub_turn_indicators_cmd_;
rclcpp::Subscription<HazardLightsCommand>::SharedPtr sub_hazard_lights_cmd_;
rclcpp::Subscription<VehicleControlCommand>::SharedPtr sub_vehicle_cmd_;
rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_ackermann_cmd_;
rclcpp::Subscription<AckermannControlCommand>::SharedPtr sub_manual_ackermann_cmd_;
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<Control>::SharedPtr sub_ackermann_cmd_;
rclcpp::Subscription<Control>::SharedPtr sub_manual_ackermann_cmd_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_init_pose_;
rclcpp::Subscription<TwistStamped>::SharedPtr sub_init_twist_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
Expand Down Expand Up @@ -176,8 +170,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
VelocityReport current_velocity_{};
Odometry current_odometry_{};
SteeringReport current_steer_{};
AckermannControlCommand current_ackermann_cmd_{};
AckermannControlCommand current_manual_ackermann_cmd_{};
Control current_ackermann_cmd_{};
Control current_manual_ackermann_cmd_{};
GearCommand current_gear_cmd_{};
GearCommand current_manual_gear_cmd_{};
TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{};
Expand Down Expand Up @@ -215,15 +209,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
} vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics
std::shared_ptr<SimModelInterface> vehicle_model_ptr_; //!< @brief vehicle model pointer

/**
* @brief set current_vehicle_cmd_ptr_ with received message
*/
void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg);

/**
* @brief set input steering, velocity, and acceleration of the vehicle model
*/
void set_input(const AckermannControlCommand & cmd, const double acc_by_slope);
void set_input(const Control & cmd, const double acc_by_slope);

/**
* @brief set current_vehicle_state_ with received message
Expand All @@ -238,7 +227,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
/**
* @brief subscribe lanelet map
*/
void on_map(const HADMapBin::ConstSharedPtr msg);
void on_map(const LaneletMapBin::ConstSharedPtr msg);

/**
* @brief set initial pose for simulation with received message
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface
* @brief update state considering current gear
* @param [in] state current state
* @param [in] prev_state previous state
* @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand)
* @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand)
* @param [in] dt delta time to update state
*/
void updateStateWithGear(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface
* @brief update state considering current gear
* @param [in] state current state
* @param [in] prev_state previous state
* @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand)
* @param [in] gear current gear (defined in autoware_msgs/GearCommand)
* @param [in] dt delta time to update state
*/
void updateStateWithGear(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface
* @brief update state considering current gear
* @param [in] state current state
* @param [in] prev_state previous state
* @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand)
* @param [in] gear current gear (defined in autoware_vehicle_msgs/GearCommand)
* @param [in] dt delta time to update state
*/
void updateStateWithGear(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include <Eigen/Core>

#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
#include "autoware_vehicle_msgs/msg/gear_command.hpp"

/**
* @class SimModelInterface
Expand All @@ -31,8 +31,8 @@ class SimModelInterface
Eigen::VectorXd state_; //!< @brief vehicle state vector
Eigen::VectorXd input_; //!< @brief vehicle input vector

//!< @brief gear command defined in autoware_auto_msgs/GearCommand
uint8_t gear_ = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE;
//!< @brief gear command defined in autoware_vehicle_msgs/GearCommand
uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE;

public:
/**
Expand Down Expand Up @@ -73,7 +73,7 @@ class SimModelInterface

/**
* @brief set gear
* @param [in] gear gear command defined in autoware_auto_msgs/GearCommand
* @param [in] gear gear command defined in autoware_vehicle_msgs/GearCommand
*/
void setGear(const uint8_t gear);

Expand Down
11 changes: 5 additions & 6 deletions simulator/simple_planning_simulator/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,12 +15,10 @@
<buildtool_depend>ament_cmake_auto</buildtool_depend>
<buildtool_depend>autoware_cmake</buildtool_depend>

<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_control_msgs</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>autoware_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>lanelet2_core</depend>
<depend>lanelet2_extension</depend>
Expand All @@ -38,6 +36,7 @@
<depend>tier4_external_api_msgs</depend>
<depend>tier4_vehicle_msgs</depend>
<depend>vehicle_info_util</depend>
<build_depend>autoware_cmake</build_depend>

<exec_depend>launch_ros</exec_depend>

Expand Down
Loading

0 comments on commit ead0068

Please sign in to comment.