From 164fcd62c5650a3fdd1d14bb8d532e30542fe1e5 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 29 May 2023 11:24:27 +0900 Subject: [PATCH 01/11] build(iron): add missing dependencies for centerpoint (#3824) Signed-off-by: wep21 --- perception/lidar_centerpoint/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 49f51a648dcf1..957aa147c41c4 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -21,6 +21,7 @@ tensorrt_common tf2_eigen tf2_geometry_msgs + tf2_sensor_msgs tier4_autoware_utils ament_lint_auto From 527aaf76f685a893feb277f2d88e264f8ace2c2c Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 29 May 2023 11:30:55 +0900 Subject: [PATCH 02/11] build(iron): add workaroud for rosbag2 storage plugin update (#3822) Signed-off-by: wep21 --- perception/elevation_map_loader/CMakeLists.txt | 8 ++++++++ .../src/elevation_map_loader_node.cpp | 5 +++++ 2 files changed, 13 insertions(+) diff --git a/perception/elevation_map_loader/CMakeLists.txt b/perception/elevation_map_loader/CMakeLists.txt index da153466ba8fe..6bfe60ef21062 100644 --- a/perception/elevation_map_loader/CMakeLists.txt +++ b/perception/elevation_map_loader/CMakeLists.txt @@ -11,6 +11,14 @@ ament_auto_add_library(elevation_map_loader_node SHARED ) target_link_libraries(elevation_map_loader_node ${PCL_LIBRARIES}) +# TODO(wep21): workaround for iron. +# remove this block and update package.xml after iron. +find_package(rosbag2_storage_sqlite3) +target_include_directories(elevation_map_loader_node + PRIVATE + ${rosbag2_storage_sqlite3_INCLUDE_DIRS} +) + rclcpp_components_register_node(elevation_map_loader_node PLUGIN "ElevationMapLoaderNode" EXECUTABLE elevation_map_loader diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 351493d24bf61..ff3d998aabc0b 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -46,7 +46,12 @@ #define EIGEN_MPL2_ONLY #include #include + +#if __has_include() +#include +#else #include +#endif ElevationMapLoaderNode::ElevationMapLoaderNode(const rclcpp::NodeOptions & options) : Node("elevation_map_loader", options) From e8fb8454dcc33e3f2952ab680759f4e2b6e33d27 Mon Sep 17 00:00:00 2001 From: Takahiro Ishikawa Date: Mon, 29 May 2023 13:24:47 +0900 Subject: [PATCH 03/11] fix(distortion_corrector_node): fix dereference of uninitialized iterator (#3849) fix dereference of uninitialized iterator Signed-off-by: Takahiro Ishikawa --- .../src/distortion_corrector/distortion_corrector.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index 36a9b0c632612..dd38b85a2b56d 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -222,7 +222,6 @@ bool DistortionCorrectorComponent::undistortPointCloud( // For performance, do not instantiate `rclcpp::Time` inside of the for-loop double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); - double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); // For performance, instantiate outside of the for-loop tf2::Quaternion baselink_quat{}; @@ -251,6 +250,9 @@ bool DistortionCorrectorComponent::undistortPointCloud( } if (use_imu_ && !angular_velocity_queue_.empty()) { + // For performance, do not instantiate `rclcpp::Time` inside of the for-loop + double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); + for (; (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds()); From 451b504b0a4b1e4ffa09b1afb05fc2786cf98fbb Mon Sep 17 00:00:00 2001 From: yabuta Date: Mon, 29 May 2023 15:18:29 +0900 Subject: [PATCH 04/11] feat(autoware_api_specs): change topic depth of route state api and localization state api (#3757) fix depth --- .../include/autoware_ad_api_specs/localization.hpp | 2 +- .../include/autoware_ad_api_specs/routing.hpp | 2 +- .../include/component_interface_specs/localization.hpp | 2 +- .../include/component_interface_specs/planning.hpp | 2 +- common/component_interface_utils/README.md | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp index a2db70fb8cb7a..634713845868b 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/localization.hpp @@ -33,7 +33,7 @@ struct InitializationState { using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; static constexpr char name[] = "/api/localization/initialization_state"; - static constexpr size_t depth = 3; + static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp index 62f99a4a8d8e9..1e119bc36e9b7 100644 --- a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/routing.hpp @@ -60,7 +60,7 @@ struct RouteState { using Message = autoware_adapi_v1_msgs::msg::RouteState; static constexpr char name[] = "/api/routing/state"; - static constexpr size_t depth = 3; + static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; diff --git a/common/component_interface_specs/include/component_interface_specs/localization.hpp b/common/component_interface_specs/include/component_interface_specs/localization.hpp index 488a18b40c99c..89740ffbb1f4c 100644 --- a/common/component_interface_specs/include/component_interface_specs/localization.hpp +++ b/common/component_interface_specs/include/component_interface_specs/localization.hpp @@ -34,7 +34,7 @@ struct InitializationState { using Message = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; static constexpr char name[] = "/localization/initialization_state"; - static constexpr size_t depth = 3; + static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index efea12bb0e908..c2f2bb8c15eb7 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -61,7 +61,7 @@ struct RouteState { using Message = autoware_adapi_v1_msgs::msg::RouteState; static constexpr char name[] = "/planning/mission_planning/route_state"; - static constexpr size_t depth = 3; + static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; diff --git a/common/component_interface_utils/README.md b/common/component_interface_utils/README.md index 23d2c9ed1fc20..2a2b191c854a0 100644 --- a/common/component_interface_utils/README.md +++ b/common/component_interface_utils/README.md @@ -30,7 +30,7 @@ struct SampleMessage { using Message = sample_msgs::msg::MessageType; static constexpr char name[] = "/sample/message"; - static constexpr size_t depth = 3; + static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; From 94d2d5cdf1c3b5e78437255b2632701f693e0222 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 29 May 2023 15:34:52 +0900 Subject: [PATCH 05/11] fix(goal_planner): generate goal candidates after execution (#3854) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 48 ++++++++++--------- 2 files changed, 27 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 7ea0f6d245837..1156dd2dfc5e4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -185,6 +185,7 @@ class GoalPlannerModule : public SceneModuleInterface // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; + void generateGoalCandidates(); // stop or decelerate void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e1cc7d0354283..07fe7f310c05a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -258,32 +258,10 @@ void GoalPlannerModule::initializeOccupancyGridMap() void GoalPlannerModule::processOnEntry() { - const auto & route_handler = planner_data_->route_handler; - // Initialize occupancy grid map if (parameters_->use_occupancy_grid) { initializeOccupancyGridMap(); } - - // initialize when receiving new route - if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { - // Initialize parallel parking planner status - resetStatus(); - - // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); - refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (allow_goal_modification_) { - goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); - } else { - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = goal_pose; - goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); - } - } - last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); } void GoalPlannerModule::processOnExit() @@ -486,8 +464,34 @@ void GoalPlannerModule::returnToLaneParking() RCLCPP_INFO(getLogger(), "return to lane parking"); } +void GoalPlannerModule::generateGoalCandidates() +{ + // initialize when receiving new route + const auto & route_handler = planner_data_->route_handler; + if (!last_received_time_ || *last_received_time_ != route_handler->getRouteHeader().stamp) { + // Initialize parallel parking planner status + resetStatus(); + + // calculate goal candidates + const Pose goal_pose = route_handler->getGoalPose(); + refined_goal_pose_ = calcRefinedGoal(goal_pose); + if (allow_goal_modification_) { + goal_searcher_->setPlannerData(planner_data_); + goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + } else { + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = goal_pose; + goal_candidate.distance_from_original_goal = 0.0; + goal_candidates_.push_back(goal_candidate); + } + } + last_received_time_ = std::make_unique(route_handler->getRouteHeader().stamp); +} + BehaviorModuleOutput GoalPlannerModule::plan() { + generateGoalCandidates(); + if (allow_goal_modification_) { return planWithGoalModification(); } else { From 802c392090b510826ab4e982610dd9eac7566abf Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 29 May 2023 16:48:23 +0900 Subject: [PATCH 06/11] feat(mission_planner): add guard for reroute (#3841) --- .../src/mission_planner/mission_planner.cpp | 113 ++++++++++++++---- .../src/mission_planner/mission_planner.hpp | 7 ++ 2 files changed, 96 insertions(+), 24 deletions(-) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index b3df2d0a0010a..b97f9d0d77e31 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -170,59 +170,78 @@ void MissionPlanner::change_route(const LaneletRoute & route) normal_route_ = std::make_shared(route); } -LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) +LaneletRoute MissionPlanner::create_route( + const std_msgs::msg::Header & header, + const std::vector & route_segments, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) { - PoseStamped goal_pose; - goal_pose.header = req->header; - goal_pose.pose = req->goal; + PoseStamped goal_pose_stamped; + goal_pose_stamped.header = header; + goal_pose_stamped.pose = goal_pose; // Convert route. LaneletRoute route; route.start_pose = odometry_->pose.pose; - route.goal_pose = transform_pose(goal_pose).pose; - for (const auto & segment : req->segments) { + route.goal_pose = transform_pose(goal_pose_stamped).pose; + for (const auto & segment : route_segments) { route.segments.push_back(convert(segment)); } - route.header.stamp = req->header.stamp; + route.header.stamp = header.stamp; route.header.frame_id = map_frame_; route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + route.allow_modification = allow_goal_modification; return route; } -LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) +LaneletRoute MissionPlanner::create_route( + const std_msgs::msg::Header & header, const std::vector & waypoints, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; - // Use temporary pose stamped for transform. PoseStamped pose; - pose.header = req->header; + pose.header = header; // Convert route points. PlannerPlugin::RoutePoints points; points.push_back(odometry_->pose.pose); - for (const auto & waypoint : req->waypoints) { + for (const auto & waypoint : waypoints) { pose.pose = waypoint; points.push_back(transform_pose(pose).pose); } - pose.pose = req->goal; + pose.pose = goal_pose; points.push_back(transform_pose(pose).pose); // Plan route. LaneletRoute route = planner_->plan(points); - if (route.segments.empty()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } - route.header.stamp = req->header.stamp; + route.header.stamp = header.stamp; route.header.frame_id = map_frame_; route.uuid.uuid = generate_random_id(); - route.allow_modification = req->option.allow_goal_modification; + route.allow_modification = allow_goal_modification; return route; } +LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) +{ + const auto & header = req->header; + const auto & route_segments = req->segments; + const auto & goal_pose = req->goal; + const auto & allow_goal_modification = req->option.allow_goal_modification; + + return create_route(header, route_segments, goal_pose, allow_goal_modification); +} + +LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) +{ + const auto & header = req->header; + const auto & waypoints = req->waypoints; + const auto & goal_pose = req->goal; + const auto & allow_goal_modification = req->option.allow_goal_modification; + + return create_route(header, waypoints, goal_pose, allow_goal_modification); +} + void MissionPlanner::change_state(RouteState::Message::_state_type state) { state_.stamp = now(); @@ -249,6 +268,10 @@ void MissionPlanner::on_set_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_ROUTE_EXISTS, "The route is already set."); } + if (!planner_->ready()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } if (!odometry_) { throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); @@ -257,6 +280,12 @@ void MissionPlanner::on_set_route( // Convert request to a new route. const auto route = create_route(req); + // Check planned routes + if (route.segments.empty()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // Update route. change_route(route); change_state(RouteState::Message::SET); @@ -287,6 +316,12 @@ void MissionPlanner::on_set_route_points( // Plan route. const auto route = create_route(req); + // Check planned routes + if (route.segments.empty()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // Update route. change_route(route); change_state(RouteState::Message::SET); @@ -329,10 +364,18 @@ void MissionPlanner::on_change_route( throw component_interface_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); } + if (!planner_->ready()) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + } if (!odometry_) { throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (!normal_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); + } // set to changing state change_state(RouteState::Message::CHANGING); @@ -340,17 +383,26 @@ void MissionPlanner::on_change_route( // Convert request to a new route. const auto new_route = create_route(req); + // Check planned routes + if (new_route.segments.empty()) { + change_route(*normal_route_); + change_state(RouteState::Message::SET); + res->status.success = false; + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { // sucess to reroute change_route(new_route); - change_state(RouteState::Message::SET); res->status.success = true; + change_state(RouteState::Message::SET); } else { // failed to reroute change_route(*normal_route_); - change_state(RouteState::Message::SET); res->status.success = false; + change_state(RouteState::Message::SET); throw component_interface_utils::ServiceException( ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } @@ -375,23 +427,36 @@ void MissionPlanner::on_change_route_points( throw component_interface_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } + if (!normal_route_) { + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); + } change_state(RouteState::Message::CHANGING); // Plan route. const auto new_route = create_route(req); + // Check planned routes + if (new_route.segments.empty()) { + change_state(RouteState::Message::SET); + change_route(*normal_route_); + res->status.success = false; + throw component_interface_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + } + // check route safety if (checkRerouteSafety(*normal_route_, new_route)) { // sucess to reroute change_route(new_route); - change_state(RouteState::Message::SET); res->status.success = true; + change_state(RouteState::Message::SET); } else { // failed to reroute change_route(*normal_route_); - change_state(RouteState::Message::SET); res->status.success = false; + change_state(RouteState::Message::SET); throw component_interface_utils::ServiceException( ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index f108e2d99496c..a06398bd8d84f 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -78,6 +78,13 @@ class MissionPlanner : public rclcpp::Node void change_route(const LaneletRoute & route); LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req); LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req); + LaneletRoute create_route( + const std_msgs::msg::Header & header, + const std::vector & route_segments, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); + LaneletRoute create_route( + const std_msgs::msg::Header & header, const std::vector & waypoints, + const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); RouteState::Message state_; component_interface_utils::Publisher::SharedPtr pub_state_; From 4e45188f451e3c410b766724973e2c180cff9310 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Mon, 29 May 2023 18:24:45 +0900 Subject: [PATCH 07/11] feat(tier4_adapi_rviz_plugin): add route panel (#3840) * feat: add panel Signed-off-by: Takagi, Isamu * feat: set route Signed-off-by: Takagi, Isamu * feat: set waypoints Signed-off-by: Takagi, Isamu * feat: add readme Signed-off-by: Takagi, Isamu * fix: copyright Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- common/tier4_adapi_rviz_plugin/CMakeLists.txt | 2 +- common/tier4_adapi_rviz_plugin/README.md | 11 ++ .../icons/classes/RoutePanel.png | Bin 0 -> 18815 bytes common/tier4_adapi_rviz_plugin/package.xml | 3 + .../plugins/plugin_description.xml | 4 + .../src/route_panel.cpp | 140 ++++++++++++++++++ .../src/route_panel.hpp | 67 +++++++++ 7 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 common/tier4_adapi_rviz_plugin/README.md create mode 100644 common/tier4_adapi_rviz_plugin/icons/classes/RoutePanel.png create mode 100644 common/tier4_adapi_rviz_plugin/src/route_panel.cpp create mode 100644 common/tier4_adapi_rviz_plugin/src/route_panel.hpp diff --git a/common/tier4_adapi_rviz_plugin/CMakeLists.txt b/common/tier4_adapi_rviz_plugin/CMakeLists.txt index 7b3f909115dff..b756e4686f82f 100644 --- a/common/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/common/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -8,10 +8,10 @@ find_package(Qt5 REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) set(CMAKE_AUTOMOC ON) set(CMAKE_INCLUDE_CURRENT_DIR ON) -add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp + src/route_panel.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/common/tier4_adapi_rviz_plugin/README.md b/common/tier4_adapi_rviz_plugin/README.md new file mode 100644 index 0000000000000..41ef7c6b6243b --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/README.md @@ -0,0 +1,11 @@ +# tier4_adapi_rviz_plugin + +## RoutePanel + +To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`. +By default, when a tool publish a pose, the panel immediately sets a route with that as the goal. +Enable or disable of allow_goal_modification option can be set with the check box. + +Push the mode button in the waypoint to enter waypoint mode. In this mode, the pose is added to waypoints. +Press the apply button to set the route using the saved waypoints (the last one is a goal). +Reset the saved waypoints with the reset button. diff --git a/common/tier4_adapi_rviz_plugin/icons/classes/RoutePanel.png b/common/tier4_adapi_rviz_plugin/icons/classes/RoutePanel.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/tier4_adapi_rviz_plugin/package.xml b/common/tier4_adapi_rviz_plugin/package.xml index 1645f30e43cd4..19f8857984354 100644 --- a/common/tier4_adapi_rviz_plugin/package.xml +++ b/common/tier4_adapi_rviz_plugin/package.xml @@ -12,6 +12,9 @@ ament_cmake_auto autoware_cmake + autoware_ad_api_specs + component_interface_utils + geometry_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index ab2c0243282c0..02b18e5a51988 100644 --- a/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,8 @@ RouteTool + + RoutePanel + + diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.cpp b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp new file mode 100644 index 0000000000000..3e99761fd9fb3 --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.cpp @@ -0,0 +1,140 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "route_panel.hpp" + +#include +#include +#include + +#include + +namespace tier4_adapi_rviz_plugins +{ + +RoutePanel::RoutePanel(QWidget * parent) : rviz_common::Panel(parent) +{ + waypoints_mode_ = new QPushButton("mode"); + waypoints_reset_ = new QPushButton("reset"); + waypoints_apply_ = new QPushButton("apply"); + allow_goal_modification_ = new QCheckBox("allow goal modification"); + + waypoints_mode_->setCheckable(true); + waypoints_reset_->setDisabled(true); + waypoints_apply_->setDisabled(true); + connect(waypoints_mode_, &QPushButton::clicked, this, &RoutePanel::onWaypointsMode); + connect(waypoints_reset_, &QPushButton::clicked, this, &RoutePanel::onWaypointsReset); + connect(waypoints_apply_, &QPushButton::clicked, this, &RoutePanel::onWaypointsApply); + + const auto layout = new QVBoxLayout(); + setLayout(layout); + + // waypoints group + { + const auto group = new QGroupBox("waypoints"); + const auto local = new QHBoxLayout(); + local->addWidget(waypoints_mode_); + local->addWidget(waypoints_reset_); + local->addWidget(waypoints_apply_); + group->setLayout(local); + layout->addWidget(group); + waypoints_group_ = group; + } + + // options group + { + const auto group = new QGroupBox("options"); + const auto local = new QHBoxLayout(); + local->addWidget(allow_goal_modification_); + group->setLayout(local); + layout->addWidget(group); + } +} + +void RoutePanel::onInitialize() +{ + auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); + auto node = lock->get_raw_node(); + + sub_pose_ = node->create_subscription( + "/rviz/routing/pose", rclcpp::QoS(1), + std::bind(&RoutePanel::onPose, this, std::placeholders::_1)); + + const auto adaptor = component_interface_utils::NodeAdaptor(node.get()); + adaptor.init_cli(cli_clear_); + adaptor.init_cli(cli_route_); +} + +void RoutePanel::setRoute(const PoseStamped & pose) +{ + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [this, pose](auto) { + const auto req = std::make_shared(); + req->header = pose.header; + req->goal = pose.pose; + req->option.allow_goal_modification = allow_goal_modification_->isChecked(); + cli_route_->async_send_request(req); + }); +} + +void RoutePanel::onPose(const PoseStamped::ConstSharedPtr msg) +{ + if (!waypoints_mode_->isChecked()) { + setRoute(*msg); + } else { + waypoints_.push_back(*msg); + waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size())); + } +} + +void RoutePanel::onWaypointsMode(bool clicked) +{ + waypoints_reset_->setEnabled(clicked); + waypoints_apply_->setEnabled(clicked); + + if (clicked) { + onWaypointsReset(); + } else { + waypoints_group_->setTitle("waypoints"); + } +} + +void RoutePanel::onWaypointsReset() +{ + waypoints_.clear(); + waypoints_group_->setTitle(QString("waypoints (count: %1)").arg(waypoints_.size())); +} + +void RoutePanel::onWaypointsApply() +{ + if (waypoints_.empty()) return; + + const auto req = std::make_shared(); + cli_clear_->async_send_request(req, [this](auto) { + const auto req = std::make_shared(); + req->header = waypoints_.back().header; + req->goal = waypoints_.back().pose; + for (size_t i = 0; i + 1 < waypoints_.size(); ++i) { + req->waypoints.push_back(waypoints_[i].pose); + } + req->option.allow_goal_modification = allow_goal_modification_->isChecked(); + cli_route_->async_send_request(req); + onWaypointsReset(); + }); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::RoutePanel, rviz_common::Panel) diff --git a/common/tier4_adapi_rviz_plugin/src/route_panel.hpp b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp new file mode 100644 index 0000000000000..f7c953d65dd2d --- /dev/null +++ b/common/tier4_adapi_rviz_plugin/src/route_panel.hpp @@ -0,0 +1,67 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROUTE_PANEL_HPP_ +#define ROUTE_PANEL_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace tier4_adapi_rviz_plugins +{ + +class RoutePanel : public rviz_common::Panel +{ + Q_OBJECT + using ClearRoute = autoware_ad_api::routing::ClearRoute; + using SetRoutePoints = autoware_ad_api::routing::SetRoutePoints; + using PoseStamped = geometry_msgs::msg::PoseStamped; + +public: + explicit RoutePanel(QWidget * parent = nullptr); + void onInitialize() override; + +private: + QPushButton * waypoints_mode_; + QPushButton * waypoints_reset_; + QPushButton * waypoints_apply_; + QGroupBox * waypoints_group_; + QCheckBox * allow_goal_modification_; + + rclcpp::Subscription::SharedPtr sub_pose_; + std::vector waypoints_; + void onPose(const PoseStamped::ConstSharedPtr msg); + + component_interface_utils::Client::SharedPtr cli_clear_; + component_interface_utils::Client::SharedPtr cli_route_; + void setRoute(const PoseStamped & pose); + +private slots: + void onWaypointsMode(bool clicked); + void onWaypointsReset(); + void onWaypointsApply(); +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // ROUTE_PANEL_HPP_ From 9ee5d78634aabe830ab33b2815ad08e8ee0a6724 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 29 May 2023 18:43:55 +0900 Subject: [PATCH 08/11] fix(behavior_path_planner): check previous lanes in isEgoOutOfRoute (#3856) Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utils/utils.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 879aa1803df9b..e6b6cbad6e33c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1090,7 +1090,20 @@ bool isEgoOutOfRoute( "cannot find closest road lanelet"); return false; } - return lanelet::utils::isInLanelet(self_pose, closest_road_lane); + + if (lanelet::utils::isInLanelet(self_pose, closest_road_lane)) { + return true; + } + + // check previous lanes for backward driving (e.g. pull out) + const auto prev_lanes = route_handler->getPreviousLanelets(closest_road_lane); + for (const auto & lane : prev_lanes) { + if (lanelet::utils::isInLanelet(self_pose, lane)) { + return true; + } + } + + return false; }); if (!is_in_shoulder_lane && !is_in_road_lane) { return true; From db5aba375393d5f26ffb07b6910d8b459893c2a6 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 29 May 2023 23:50:27 +0900 Subject: [PATCH 09/11] fix(map_loader): re-align lanelet borders after overwriting coordinates (#3825) Signed-off-by: Ryohsuke Mitsudome --- .../lanelet2_map_loader/lanelet2_map_loader_node.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 be8ffc94bc296..110fff125ffbb 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 @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -107,6 +108,16 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( point.y() = point.attribute("local_y").asDouble().value(); } } + + // realign lanelet borders using updated points + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + return map; } else { RCLCPP_ERROR(rclcpp::get_logger("map_loader"), "lanelet2_map_projector_type is not supported"); From 4856894a7547e6df16f077cf779a07374fd2bc97 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 30 May 2023 01:21:47 +0300 Subject: [PATCH 10/11] feat(vehicle_cmd_gate): add moderate_stop_interface to make vehicle stop (#3790) * feat(vehicle_cmd_gate): add moderate_stop_interface to make vehicle stop Signed-off-by: Berkay Karaman Signed-off-by: Berkay Karaman * move filter place Signed-off-by: Berkay Karaman --------- Signed-off-by: Berkay Karaman --- .../component_interface_specs/control.hpp | 17 +++++ control/vehicle_cmd_gate/CMakeLists.txt | 1 + control/vehicle_cmd_gate/README.md | 1 + .../config/vehicle_cmd_gate.param.yaml | 1 + .../src/moderate_stop_interface.cpp | 69 +++++++++++++++++++ .../src/moderate_stop_interface.hpp | 60 ++++++++++++++++ .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 10 +++ .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 3 + 8 files changed, 162 insertions(+) create mode 100644 control/vehicle_cmd_gate/src/moderate_stop_interface.cpp create mode 100644 control/vehicle_cmd_gate/src/moderate_stop_interface.hpp diff --git a/common/component_interface_specs/include/component_interface_specs/control.hpp b/common/component_interface_specs/include/component_interface_specs/control.hpp index ac99135eb6ef6..dfa708dd33403 100644 --- a/common/component_interface_specs/include/component_interface_specs/control.hpp +++ b/common/component_interface_specs/include/component_interface_specs/control.hpp @@ -19,7 +19,9 @@ #include #include +#include #include +#include namespace control_interface { @@ -48,6 +50,21 @@ struct IsStartRequested static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; +struct SetStop +{ + using Service = tier4_control_msgs::srv::SetStop; + static constexpr char name[] = "/control/vehicle_cmd_gate/set_stop"; +}; + +struct IsStopped +{ + using Message = tier4_control_msgs::msg::IsStopped; + static constexpr char name[] = "/control/vehicle_cmd_gate/is_stopped"; + static constexpr size_t depth = 1; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; +}; + } // namespace control_interface #endif // COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/vehicle_cmd_gate/CMakeLists.txt index f009bb1af52a2..3cb7f560d1277 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/control/vehicle_cmd_gate/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED src/vehicle_cmd_gate.cpp src/vehicle_cmd_filter.cpp src/pause_interface.cpp + src/moderate_stop_interface.cpp ) rclcpp_components_register_node(vehicle_cmd_gate_node diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 4bae58e22fa93..a8ac8c46ae782 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -52,6 +52,7 @@ | `external_emergency_stop_heartbeat_timeout` | double | timeout for external emergency | | `stop_hold_acceleration` | double | longitudinal acceleration cmd when vehicle should stop | | `emergency_acceleration` | double | longitudinal acceleration cmd when vehicle stop with emergency | +| `moderate_stop_service_acceleration` | double | longitudinal acceleration cmd when vehicle stop with moderate stop service | | `nominal.vel_lim` | double | limit of longitudinal velocity (activated in AUTONOMOUS operation mode) | | `nominal.lon_acc_lim` | double | limit of longitudinal acceleration (activated in AUTONOMOUS operation mode) | | `nominal.lon_jerk_lim` | double | limit of longitudinal jerk (activated in AUTONOMOUS operation mode) | diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index dc93e0848e866..9fe692e16d962 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -8,6 +8,7 @@ external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 + moderate_stop_service_acceleration: -1.5 stopped_state_entry_duration_time: 0.1 nominal: vel_lim: 25.0 diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp new file mode 100644 index 0000000000000..72dbdfd26a6b3 --- /dev/null +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp @@ -0,0 +1,69 @@ +// Copyright 2023 LeoDrive, A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "moderate_stop_interface.hpp" + +namespace vehicle_cmd_gate +{ + +ModerateStopInterface::ModerateStopInterface(rclcpp::Node * node) : node_(node) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(node); + adaptor.init_srv(srv_set_stop_, this, &ModerateStopInterface::on_stop_request); + adaptor.init_pub(pub_is_stopped_); + + stop_state_.data = false; + stop_state_.requested_sources.clear(); + publish(); +} + +bool ModerateStopInterface::is_stop_requested() const +{ + return stop_state_.data; +} + +void ModerateStopInterface::publish() +{ + if (prev_stop_map_ != stop_map_) { + pub_is_stopped_->publish(stop_state_); + prev_stop_map_ = stop_map_; + } +} + +void ModerateStopInterface::on_stop_request( + const SetStop::Service::Request::SharedPtr req, const SetStop::Service::Response::SharedPtr res) +{ + stop_map_[req->request_source] = req->stop; + update_stop_state(); + res->status.success = true; +} + +void ModerateStopInterface::update_stop_state() +{ + stop_state_.stamp = node_->now(); + stop_state_.requested_sources.clear(); + stop_state_.data = false; + + if (stop_map_.empty()) { + return; + } + for (auto & itr : stop_map_) { + if (itr.second) { + stop_state_.data = true; + stop_state_.requested_sources.push_back(itr.first); + } + } +} + +} // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp new file mode 100644 index 0000000000000..7ef30b4926b7e --- /dev/null +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -0,0 +1,60 @@ +// Copyright 2023 LeoDrive, A.Ş. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MODERATE_STOP_INTERFACE_HPP_ +#define MODERATE_STOP_INTERFACE_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace vehicle_cmd_gate +{ + +class ModerateStopInterface +{ +private: + using SetStop = control_interface::SetStop; + using IsStopped = control_interface::IsStopped; + using IsStartRequested = control_interface::IsStartRequested; + +public: + explicit ModerateStopInterface(rclcpp::Node * node); + bool is_stop_requested() const; + void publish(); + +private: + IsStopped::Message stop_state_; + std::unordered_map stop_map_; + std::optional> prev_stop_map_; + + rclcpp::Node * node_; + component_interface_utils::Service::SharedPtr srv_set_stop_; + component_interface_utils::Publisher::SharedPtr pub_is_stopped_; + + void on_stop_request( + const SetStop::Service::Request::SharedPtr req, + const SetStop::Service::Response::SharedPtr res); + + void update_stop_state(); +}; + +} // namespace vehicle_cmd_gate + +#endif // MODERATE_STOP_INTERFACE_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index b5609b1088530..aa80a28c687b9 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -143,6 +143,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("external_emergency_stop_heartbeat_timeout"); stop_hold_acceleration_ = declare_parameter("stop_hold_acceleration"); emergency_acceleration_ = declare_parameter("emergency_acceleration"); + moderate_stop_service_acceleration_ = + declare_parameter("moderate_stop_service_acceleration"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -196,6 +198,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Pause interface pause_ = std::make_unique(this); + moderate_stop_interface_ = std::make_unique(this); // Timer const auto update_period = 1.0 / declare_parameter("update_rate"); @@ -378,6 +381,11 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) filtered_commands.gear = commands.gear; // tmp } + if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle + filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_; + } + // Check emergency if (use_emergency_handling_ && is_system_emergency_) { RCLCPP_WARN_THROTTLE( @@ -410,6 +418,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency); control_cmd_pub_->publish(filtered_commands.control); pause_->publish(); + moderate_stop_interface_->publish(); // Save ControlCmd to steering angle when disengaged prev_control_cmd_ = filtered_commands.control; @@ -474,6 +483,7 @@ void VehicleCmdGate::publishStatus() pub_external_emergency_->publish(external_emergency); operation_mode_pub_->publish(current_operation_mode_); pause_->publish(); + moderate_stop_interface_->publish(); } AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index ee74bbdb90acf..b22d1448d77fa 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -15,6 +15,7 @@ #ifndef VEHICLE_CMD_GATE_HPP_ #define VEHICLE_CMD_GATE_HPP_ +#include "moderate_stop_interface.hpp" #include "pause_interface.hpp" #include "vehicle_cmd_filter.hpp" @@ -160,6 +161,7 @@ class VehicleCmdGate : public rclcpp::Node double external_emergency_stop_heartbeat_timeout_; double stop_hold_acceleration_; double emergency_acceleration_; + double moderate_stop_service_acceleration_; // Service rclcpp::Service::SharedPtr srv_engage_; @@ -216,6 +218,7 @@ class VehicleCmdGate : public rclcpp::Node // Pause interface for API std::unique_ptr pause_; + std::unique_ptr moderate_stop_interface_; }; } // namespace vehicle_cmd_gate From 0eb24846148757d59c510eeb3235f906bf1c1819 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 30 May 2023 10:03:48 +0900 Subject: [PATCH 11/11] feat(dummy_perception_publisher): publish ground truth object in option (#3731) * add ground truth publisher Signed-off-by: yoshiri * refactor ground truth publisher Signed-off-by: yoshiri * enable to fix random seed Signed-off-by: yoshiri * update readme Signed-off-by: yoshiri * change GT output name to debug info Signed-off-by: yoshiri * debug info must be under the node ns Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../dummy_perception_publisher/README.md | 12 ++-- .../dummy_perception_publisher/node.hpp | 11 ++++ .../dummy_perception_publisher.launch.xml | 2 + .../dummy_perception_publisher/src/node.cpp | 66 +++++++++++++++++-- 4 files changed, 83 insertions(+), 8 deletions(-) diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/dummy_perception_publisher/README.md index cfd5eb341ad5b..48036948ece65 100644 --- a/simulator/dummy_perception_publisher/README.md +++ b/simulator/dummy_perception_publisher/README.md @@ -17,10 +17,11 @@ This node publishes the result of the dummy detection with the type of perceptio ### Output -| Name | Type | Description | -| ----------------------- | -------------------------------------------------------- | ---------------------- | -| `output/dynamic_object` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | Publishes objects | -| `output/points_raw` | `sensor_msgs::msg::PointCloud2` | point cloud of objects | +| Name | Type | Description | +| ----------------------------------- | -------------------------------------------------------- | ----------------------- | +| `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 | ## Parameters @@ -31,6 +32,9 @@ This node publishes the result of the dummy detection with the type of perceptio | `enable_ray_tracing` | bool | true | if True, use ray tracking | | `use_object_recognition` | bool | true | if True, publish objects topic | | `use_base_link_z` | bool | true | if True, node uses z coordinate of ego base_link | +| `publish_ground_truth` | bool | false | if True, publish ground truth objects | +| `use_fixed_random_seed` | bool | false | if True, use fixed random seed | +| `random_seed` | int | 0 | random seed | ### Node 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 0c859838bbe42..770663e84dc0c 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -53,6 +54,13 @@ struct ObjectInfo double std_dev_z; double std_dev_yaw; tf2::Transform tf_map2moved_object; + // pose and twist + geometry_msgs::msg::TwistWithCovariance twist_covariance_; + 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( + const dummy_perception_publisher::msg::Object & object) const; }; class PointCloudCreator @@ -106,6 +114,8 @@ class DummyPerceptionPublisherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr detected_object_with_feature_pub_; + rclcpp::Publisher::SharedPtr + ground_truth_objects_pub_; rclcpp::Subscription::SharedPtr object_sub_; rclcpp::TimerBase::SharedPtr timer_; tf2_ros::Buffer tf_buffer_; @@ -117,6 +127,7 @@ class DummyPerceptionPublisherNode : public rclcpp::Node bool use_object_recognition_; bool use_real_param_; bool use_base_link_z_; + bool publish_ground_truth_objects_; std::unique_ptr pointcloud_creator_; double angle_increment_; diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 3184bcb234957..3257f2bb1200c 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -21,6 +21,8 @@ + + diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 41b336ba84a0b..165658c187343 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -33,6 +33,9 @@ #include #include +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + ObjectInfo::ObjectInfo( const dummy_perception_publisher::msg::Object & object, const rclcpp::Time & current_time) : length(object.shape.dimensions.x), @@ -41,7 +44,9 @@ ObjectInfo::ObjectInfo( std_dev_x(std::sqrt(object.initial_state.pose_covariance.covariance[0])), std_dev_y(std::sqrt(object.initial_state.pose_covariance.covariance[7])), std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])), - std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])) + std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35])), + twist_covariance_(object.initial_state.twist_covariance), + pose_covariance_(object.initial_state.pose_covariance) { // calculate current pose const auto & initial_pose = object.initial_state.pose_covariance.pose; @@ -53,10 +58,10 @@ ObjectInfo::ObjectInfo( const double elapsed_time = current_time.seconds() - rclcpp::Time(object.header.stamp).seconds(); double move_distance; + double current_vel = initial_vel + initial_acc * elapsed_time; if (initial_acc == 0.0) { move_distance = initial_vel * elapsed_time; } else { - double current_vel = initial_vel + initial_acc * elapsed_time; if (initial_acc < 0 && 0 < initial_vel) { current_vel = std::max(current_vel, 0.0); } @@ -97,6 +102,24 @@ ObjectInfo::ObjectInfo( ros_map2moved_object.translation.z = current_pose.position.z; ros_map2moved_object.rotation = current_pose.orientation; tf2::fromMsg(ros_map2moved_object, tf_map2moved_object); + // set twist and pose information + twist_covariance_.twist.linear.x = current_vel; + pose_covariance_.pose = current_pose; +} + +TrackedObject ObjectInfo::toTrackedObject( + const dummy_perception_publisher::msg::Object & object) const +{ + TrackedObject tracked_object; + tracked_object.kinematics.pose_with_covariance = pose_covariance_; + tracked_object.kinematics.twist_with_covariance = twist_covariance_; + tracked_object.classification.push_back(object.classification); + tracked_object.shape.type = object.shape.type; + tracked_object.shape.dimensions.x = length; + tracked_object.shape.dimensions.y = width; + tracked_object.shape.dimensions.z = height; + tracked_object.object_id = object.id; + return tracked_object; } DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() @@ -109,6 +132,10 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() use_base_link_z_ = this->declare_parameter("use_base_link_z", true); const bool object_centric_pointcloud = this->declare_parameter("object_centric_pointcloud", false); + publish_ground_truth_objects_ = this->declare_parameter("publish_ground_truth", false); + const unsigned int random_seed = + static_cast(this->declare_parameter("random_seed", 0)); + const bool use_fixed_random_seed = this->declare_parameter("use_fixed_random_seed", false); if (object_centric_pointcloud) { pointcloud_creator_ = @@ -121,9 +148,14 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() // parameters for vehicle centric point cloud generation angle_increment_ = this->declare_parameter("angle_increment", 0.25 * M_PI / 180.0); - std::random_device seed_gen; - random_generator_.seed(seed_gen()); + if (use_fixed_random_seed) { + random_generator_.seed(random_seed); + } else { + std::random_device seed_gen; + random_generator_.seed(seed_gen()); + } + // create subscriber and publisher rclcpp::QoS qos{1}; qos.transient_local(); detected_object_with_feature_pub_ = @@ -134,6 +166,13 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() "input/object", 100, std::bind(&DummyPerceptionPublisherNode::objectCallback, this, std::placeholders::_1)); + // optional ground truth publisher + if (publish_ground_truth_objects_) { + ground_truth_objects_pub_ = + this->create_publisher( + "~output/debug/ground_truth_objects", qos); + } + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&DummyPerceptionPublisherNode::timerCallback, this)); @@ -143,6 +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; geometry_msgs::msg::PoseStamped output_moved_object_pose; sensor_msgs::msg::PointCloud2 output_pointcloud_msg; std_msgs::msg::Header header; @@ -183,6 +223,18 @@ void DummyPerceptionPublisherNode::timerCallback() obj_infos.push_back(obj_info); } + // publish ground truth + // add Tracked Object + if (publish_ground_truth_objects_) { + for (auto object : objects_) { + const auto object_info = ObjectInfo(object, current_time); + TrackedObject gt_tracked_object = object_info.toTrackedObject(object); + gt_tracked_object.existence_probability = 1.0; + output_ground_truth_objects_msg.objects.push_back(gt_tracked_object); + } + } + + // publish noised detected objects pcl::PointCloud::Ptr merged_pointcloud_ptr(new pcl::PointCloud); pcl::PointCloud::Ptr detected_merged_pointcloud_ptr( new pcl::PointCloud); @@ -223,6 +275,7 @@ void DummyPerceptionPublisherNode::timerCallback() tf_base_link2noised_moved_object = tf_base_link2map * object_info.tf_map2moved_object * tf_moved_object2noised_moved_object; + // add DetectedObjectWithFeature tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; feature_object.object.classification.push_back(object.classification); feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance; @@ -262,12 +315,17 @@ void DummyPerceptionPublisherNode::timerCallback() output_dynamic_object_msg.header.stamp = current_time; output_pointcloud_msg.header.frame_id = "base_link"; output_pointcloud_msg.header.stamp = current_time; + output_ground_truth_objects_msg.header.frame_id = "map"; + output_ground_truth_objects_msg.header.stamp = current_time; // publish pointcloud_pub_->publish(output_pointcloud_msg); if (use_object_recognition_) { detected_object_with_feature_pub_->publish(output_dynamic_object_msg); } + if (publish_ground_truth_objects_) { + ground_truth_objects_pub_->publish(output_ground_truth_objects_msg); + } } void DummyPerceptionPublisherNode::objectCallback(