diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +invalidPointerCast +knownConditionTrueFalse +missingInclude +missingIncludeSystem +multiCondition +noConstructor +noExplicitConstructor +noValidConfiguration +obstacle_cruise_planner +passedByValue +preprocessorErrorDirective +redundantAssignment +redundantContinue +redundantIfRemove +redundantInitialization +returnByReference +selfAssignment +shadowArgument +shadowFunction +shadowVariable +stlFindInsert +syntaxError +uninitMemberVar +unknownMacro +unmatchedSuppression +unpreciseMathCall +unreadVariable +unsignedLessThanZero +unusedFunction +unusedScopedObject +unusedStructMember +unusedVariable +useInitializationList +useStlAlgorithm +uselessCallsSubstr +uselessOverride +variableScope +virtualCallInConstructor diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml new file mode 100644 index 0000000000000..db3bd5d259895 --- /dev/null +++ b/.github/workflows/cppcheck-all.yaml @@ -0,0 +1,60 @@ +name: cppcheck-all + +on: + pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-all: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 568c77f2509c6..c10e65cdfaab1 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,7 +18,6 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs lanelet2_extension diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 0c99d33772aea..e01201f90f7e3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl explicit Impl(rclcpp::Node * node); void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl // for fitting by vector_map_loader lanelet::LaneletMapPtr vector_map_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n } else if (fit_target_ == "vector_map") { const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_vector_map_ = node_->create_subscription( + sub_vector_map_ = node_->create_subscription( "~/vector_map", durable_qos, std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } void MapHeightFitter::Impl::on_vector_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { vector_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8a31ecee50be5..21bd39303250f 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. @@ -144,7 +144,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics -- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map ### Parameters @@ -156,7 +156,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -164,7 +164,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Subscribed Topics -- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map ### Published Topics diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 0adc7612e9f61..4e85ddec056c1 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; class Lanelet2MapLoaderNode : public rclcpp::Node @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static HADMapBin create_map_bin_msg( + static LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index e5a5fe3e3a6fa..cb640e4dc83d5 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Subscription::SharedPtr sub_map_bin_; rclcpp::Publisher::SharedPtr pub_marker_; bool viz_lanelets_centerline_; - void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a9b657f843447..4eaf8600dcab4 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,7 +19,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs component_interface_specs component_interface_utils diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 617f3dd503ce0..b097e1809a385 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -86,7 +86,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // create publisher and publish pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } @@ -141,18 +141,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( +LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index bdfbcf904cf36..c81236bec86c0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt viz_lanelets_centerline_ = true; - sub_map_bin_ = this->create_subscription( + sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt } void Lanelet2MapVisualizationNode::onMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md index 643f4233c06f0..eef36c34750ca 100644 --- a/map/map_tf_generator/README.md +++ b/map/map_tf_generator/README.md @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer #### vector_map_tf_generator -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | +| Name | Type | Description | +| ----------------- | --------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames | ### Output diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 20ca1c037a578..f242254a456a1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node map_frame_ = declare_parameter("map_frame"); viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( + sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); @@ -44,12 +44,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node private: std::string map_frame_; std::string viewer_frame_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/dummy_perception_publisher/CMakeLists.txt index 63d07faf0634c..a27667edf536b 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/dummy_perception_publisher/CMakeLists.txt @@ -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 diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/dummy_perception_publisher/README.md index 48036948ece65..5d969dd292832 100644 --- a/simulator/dummy_perception_publisher/README.md +++ b/simulator/dummy_perception_publisher/README.md @@ -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 diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 770663e84dc0c..c3f7976d3efa1 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include @@ -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; }; @@ -114,7 +114,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr detected_object_with_feature_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr ground_truth_objects_pub_; rclcpp::Subscription::SharedPtr object_sub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 30279299dbfb1..11ac1b3d39daa 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/simulator/dummy_perception_publisher/msg/Object.msg @@ -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 diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 0632bd90b2c22..4704024214d9e 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -18,7 +18,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs libpcl-all-dev pcl_conversions diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index 9b3f01fa4b267..2d1ea626fb1ac 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -14,7 +14,7 @@ #include -#include +#include #include #include @@ -24,9 +24,8 @@ class EmptyObjectsPublisher : public rclcpp::Node public: EmptyObjectsPublisher() : Node("empty_objects_publisher") { - empty_objects_pub_ = - this->create_publisher( - "~/output/objects", 1); + empty_objects_pub_ = this->create_publisher( + "~/output/objects", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -34,13 +33,12 @@ class EmptyObjectsPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr - empty_objects_pub_; + rclcpp::Publisher::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); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 3ac663d2c8647..9c58961978161 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -33,8 +33,8 @@ #include #include -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) @@ -169,7 +169,7 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() // optional ground truth publisher if (publish_ground_truth_objects_) { ground_truth_objects_pub_ = - this->create_publisher( + this->create_publisher( "~/output/debug/ground_truth_objects", qos); } @@ -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; @@ -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, diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 1f8762b722a29..4902832f836a9 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -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 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index e81d4fef00abb..18a3a3bae501a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -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" @@ -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; @@ -143,10 +138,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_; rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; - rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; - rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_manual_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_init_twist_; rclcpp::Subscription::SharedPtr sub_trajectory_; @@ -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_{}; @@ -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 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 @@ -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 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 35b95bf4b1ae5..1ecb74be41780 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -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( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 4ac91eadc0593..b83a831341ac1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -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( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index 55b5ddb8fcec5..c73cc54f4ea99 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -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( diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 981fb0f416d72..1274a1ec28a07 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" /** * @class SimModelInterface @@ -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: /** @@ -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); diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index ef80cea466277..5f04fba4fdb8e 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -15,12 +15,10 @@ ament_cmake_auto autoware_cmake - autoware_cmake - - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs + autoware_planning_msgs + autoware_vehicle_msgs geometry_msgs lanelet2_core lanelet2_extension @@ -38,6 +36,7 @@ tier4_external_api_msgs tier4_vehicle_msgs vehicle_info_util + autoware_cmake launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 2d7325d90eb89..3b65218aaf03d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -43,10 +43,10 @@ using namespace std::literals::chrono_literals; namespace { -autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( +autoware_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::VelocityReport velocity; + autoware_vehicle_msgs::msg::VelocityReport velocity; velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); velocity.lateral_velocity = 0.0F; velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); @@ -67,10 +67,10 @@ nav_msgs::msg::Odometry to_odometry( return odometry; } -autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( +autoware_vehicle_msgs::msg::SteeringReport to_steering_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::SteeringReport steer; + autoware_vehicle_msgs::msg::SteeringReport steer; steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); return steer; } @@ -108,21 +108,19 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_1; using std::placeholders::_2; - sub_map_ = create_subscription( + sub_map_ = create_subscription( "input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&SimplePlanningSimulator::on_map, this, _1)); sub_init_pose_ = create_subscription( "input/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_init_twist_ = create_subscription( "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); - sub_ackermann_cmd_ = create_subscription( + sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); - sub_manual_ackermann_cmd_ = create_subscription( + [this](const Control::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); + sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { - current_manual_ackermann_cmd_ = *msg; - }); + [this](const Control::ConstSharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; }); @@ -419,7 +417,7 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_map(const HADMapBin::ConstSharedPtr msg) +void SimplePlanningSimulator::on_map(const LaneletMapBin::ConstSharedPtr msg) { auto lanelet_map_ptr = std::make_shared(); @@ -469,14 +467,13 @@ void SimplePlanningSimulator::on_set_pose( response->status = tier4_api_utils::response_success(); } -void SimplePlanningSimulator::set_input( - const AckermannControlCommand & cmd, const double acc_by_slope) +void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by_slope) { const auto steer = cmd.lateral.steering_tire_angle; - const auto vel = cmd.longitudinal.speed; + const auto vel = cmd.longitudinal.velocity; const auto accel = cmd.longitudinal.acceleration; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); const auto gear = vehicle_model_ptr_->getGear(); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 10e6a97d703cd..26b252805db47 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -160,7 +160,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 72273f0b21ec2..fe847cba946a1 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -134,7 +134,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index b2bfe56209938..c2297f1fd1d73 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -14,7 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_vehicle_msgs/msg/gear_command.hpp" #include @@ -92,7 +92,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( state(IDX::YAW) = prev_state(IDX::YAW); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index cedfec395110e..dc49bf17a11fc 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -24,8 +24,8 @@ #include -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -51,14 +51,14 @@ class PubSubNode : public rclcpp::Node "output/odometry", rclcpp::QoS{1}, [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = - create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = create_publisher("input/initialpose", rclcpp::QoS{1}); pub_gear_cmd_ = create_publisher("input/gear_command", rclcpp::QoS{1}); } rclcpp::Subscription::SharedPtr current_odom_sub_; - rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; @@ -66,7 +66,7 @@ class PubSubNode : public rclcpp::Node }; /** - * @brief Generate an AckermannControlCommand message + * @brief Generate an Control message * @param [in] t timestamp * @param [in] steer [rad] steering * @param [in] steer_rate [rad/s] steering rotation rate @@ -74,17 +74,17 @@ class PubSubNode : public rclcpp::Node * @param [in] acc [m/s²] acceleration * @param [in] jerk [m/s3] jerk */ -AckermannControlCommand cmdGen( +Control cmdGen( const builtin_interfaces::msg::Time & t, double steer, double steer_rate, double vel, double acc, double jerk) { - AckermannControlCommand cmd; + Control cmd; cmd.stamp = t; cmd.lateral.stamp = t; cmd.lateral.steering_tire_angle = steer; cmd.lateral.steering_tire_rotation_rate = steer_rate; cmd.longitudinal.stamp = t; - cmd.longitudinal.speed = vel; + cmd.longitudinal.velocity = vel; cmd.longitudinal.acceleration = acc; cmd.longitudinal.jerk = jerk; return cmd; @@ -125,8 +125,7 @@ void sendGear( * @param [in] pub_sub_node pointer to the node used for communication */ void sendCommand( - const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, - std::shared_ptr pub_sub_node) + const Control & cmd, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) { for (int i = 0; i < 150; ++i) { pub_sub_node->pub_ackermann_command_->publish(cmd);