diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index e8661322215f..89d04bc5b3a0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -92,20 +92,23 @@ def launch_setup(context, *args, **kwargs): surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] surround_obstacle_checker_component = ComposableNode( package="surround_obstacle_checker", - plugin="SurroundObstacleCheckerNode", + plugin="surround_obstacle_checker::SurroundObstacleCheckerNode", name="surround_obstacle_checker", namespace="", remappings=[ ("~/output/no_start_reason", "/planning/scenario_planning/status/no_start_reason"), ("~/output/stop_reasons", "/planning/scenario_planning/status/stop_reasons"), - ("~/output/trajectory", "surround_obstacle_checker/trajectory"), + ("~/output/max_velocity", "/planning/scenario_planning/max_velocity_candidates"), + ( + "~/output/velocity_limit_clear_command", + "/planning/scenario_planning/clear_velocity_limit", + ), ( "~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud", ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ], parameters=[ surround_obstacle_checker_param, @@ -114,22 +117,6 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) - # relay - relay_component = ComposableNode( - package="topic_tools", - plugin="topic_tools::RelayNode", - name="skip_surround_obstacle_check_relay", - namespace="", - parameters=[ - { - "input_topic": "obstacle_avoidance_planner/trajectory", - "output_topic": "surround_obstacle_checker/trajectory", - "type": "autoware_auto_planning_msgs/msg/Trajectory", - } - ], - extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ) - # obstacle stop planner obstacle_stop_planner_param_path = os.path.join( get_package_share_directory("tier4_planning_launch"), @@ -173,7 +160,7 @@ def launch_setup(context, *args, **kwargs): ), ("~/input/objects", "/perception/object_recognition/objects"), ("~/input/odometry", "/localization/kinematic_state"), - ("~/input/trajectory", "surround_obstacle_checker/trajectory"), + ("~/input/trajectory", "obstacle_avoidance_planner/trajectory"), ], parameters=[ common_param, @@ -202,13 +189,7 @@ def launch_setup(context, *args, **kwargs): condition=IfCondition(LaunchConfiguration("use_surround_obstacle_check")), ) - relay_loader = LoadComposableNodes( - composable_node_descriptions=[relay_component], - target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_surround_obstacle_check")), - ) - - group = GroupAction([container, surround_obstacle_checker_loader, relay_loader]) + group = GroupAction([container, surround_obstacle_checker_loader]) return [group] diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 0fa8707e4fda..3d195185d9bd 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -19,22 +19,11 @@ - - - - - - - - - - - @@ -45,7 +34,7 @@ - + diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index b40b7c22c98c..ab61405c9f33 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,7 +18,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "SurroundObstacleCheckerNode" + PLUGIN "surround_obstacle_checker::SurroundObstacleCheckerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml index b57d7506b057..afcc532b7c60 100644 --- a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml +++ b/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml @@ -9,3 +9,4 @@ # ego stop state stop_state_ego_speed: 0.1 #[m/s] + stop_state_entry_duration_time: 0.1 #[s] diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index d076e26afc00..2891ac63c5df 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -25,8 +25,18 @@ #include #include +namespace surround_obstacle_checker +{ + +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + enum class PoseType : int8_t { NoStart = 0 }; enum class PointType : int8_t { NoStart = 0 }; + class SurroundObstacleCheckerDebugNode { public: @@ -38,16 +48,16 @@ class SurroundObstacleCheckerDebugNode void publish(); private: - rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + rclcpp::Publisher::SharedPtr stop_reason_pub_; double base_link2front_; - visualization_msgs::msg::MarkerArray makeVisualizationMarker(); - tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(); + MarkerArray makeVisualizationMarker(); + StopReasonArray makeStopReasonArray(); std::shared_ptr stop_obstacle_point_ptr_; std::shared_ptr stop_pose_ptr_; rclcpp::Clock::SharedPtr clock_; }; - +} // namespace surround_obstacle_checker #endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index 7e4bbb9ffd15..e437787c234a 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -16,9 +16,9 @@ #define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ #include "surround_obstacle_checker/debug_marker.hpp" -#include "tier4_autoware_utils/trajectory/tmp_conversion.hpp" #include +#include #include #include @@ -27,30 +27,31 @@ #include #include #include +#include +#include #include -#include -#include -#include -#include -#include -#include - -#include #include #include #include #include #include +#include #include -using Point2d = boost::geometry::model::d2::point_xy; -using Polygon2d = - boost::geometry::model::polygon; // counter-clockwise, open +namespace surround_obstacle_checker +{ + +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; + +using Obstacle = std::pair; enum class State { PASS, STOP }; @@ -59,70 +60,73 @@ class SurroundObstacleCheckerNode : public rclcpp::Node public: explicit SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options); + struct NodeParam + { + bool use_pointcloud; + bool use_dynamic_object; + bool is_surround_obstacle; + // For preventing chattering, + // surround_check_recover_distance_ must be bigger than surround_check_distance_ + double surround_check_recover_distance; + double surround_check_distance; + double state_clear_time; + double stop_state_ego_speed; + double stop_state_entry_duration_time; + }; + private: - void pathCallback(const Trajectory::ConstSharedPtr input_msg); - void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg); - void dynamicObjectCallback( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg); - void currentVelocityCallback(const nav_msgs::msg::Odometry::ConstSharedPtr input_msg); - void insertStopVelocity(const size_t closest_idx, TrajectoryPoints * traj); - bool convertPose( - const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target, - const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose); - bool getPose( - const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose); - void getNearestObstacle(double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - void getNearestObstacleByPointCloud( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - void getNearestObstacleByDynamicObject( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point); - bool isObstacleFound(const double min_dist_to_obj); + void onTimer(); + + void onPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + void onDynamicObjects(const PredictedObjects::ConstSharedPtr msg); + + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + boost::optional getNearestObstacle() const; + + boost::optional getNearestObstacleByPointCloud() const; + + boost::optional getNearestObstacleByDynamicObject() const; + + boost::optional getTransform( + const std::string & source, const std::string & target, const rclcpp::Time & stamp, + double duration_sec) const; + bool isStopRequired(const bool is_obstacle_found, const bool is_stopped); - size_t getClosestIdx(const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose); - bool checkStop(const TrajectoryPoint & closest_point); - Polygon2d createSelfPolygon(); - Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size); - Polygon2d createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint); - diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose); - std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose); - - /* - * ROS - */ + + bool isVehicleStopped(); + + // ros + mutable tf2_ros::Buffer tf_buffer_{get_clock()}; + mutable tf2_ros::TransformListener tf_listener_{tf_buffer_}; + rclcpp::TimerBase::SharedPtr timer_; + // publisher and subscriber - rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr pointcloud_sub_; - rclcpp::Subscription::SharedPtr - dynamic_object_sub_; - rclcpp::Subscription::SharedPtr current_velocity_sub_; - rclcpp::Publisher::SharedPtr path_pub_; - rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + rclcpp::Subscription::SharedPtr sub_pointcloud_; + rclcpp::Subscription::SharedPtr sub_dynamic_objects_; + rclcpp::Subscription::SharedPtr sub_odometry_; + rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + + // debug std::shared_ptr debug_ptr_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; // parameter - nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr_; - sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr_; + NodeParam node_param_; vehicle_info_util::VehicleInfo vehicle_info_; - Polygon2d self_poly_; - bool use_pointcloud_; - bool use_dynamic_object_; - double surround_check_distance_; - // For preventing chattering, - // surround_check_recover_distance_ must be bigger than surround_check_distance_ - double surround_check_recover_distance_; - double state_clear_time_; - double stop_state_ego_speed_; - bool is_surround_obstacle_; + + // data + nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr_; + sensor_msgs::msg::PointCloud2::ConstSharedPtr pointcloud_ptr_; + PredictedObjects::ConstSharedPtr object_ptr_; // State Machine State state_ = State::PASS; std::shared_ptr last_obstacle_found_time_; + std::shared_ptr last_running_time_; }; +} // namespace surround_obstacle_checker #endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_ diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index a0dd4b307f49..53f26a05c87c 100644 --- a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -1,24 +1,20 @@ - - - - - + + - - + + - diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 5dfed9ddb19a..b74ad2ccec25 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_auto_tf2 diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 64dbc2f349f1..8abdb67e8b2c 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,6 +14,7 @@ #include "surround_obstacle_checker/debug_marker.hpp" +#include #ifdef ROS_DISTRO_GALACTIC #include #else @@ -22,13 +23,23 @@ #include +namespace surround_obstacle_checker +{ + +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::createStopVirtualWallMarker; + SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double base_link2front, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) : base_link2front_(base_link2front), clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - stop_reason_pub_ = - node.create_publisher("~/output/stop_reasons", 1); + stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); } bool SurroundObstacleCheckerDebugNode::pushPose( @@ -70,87 +81,25 @@ void SurroundObstacleCheckerDebugNode::publish() stop_obstacle_point_ptr_ = nullptr; } -visualization_msgs::msg::MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() +MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() { - visualization_msgs::msg::MarkerArray msg; + MarkerArray msg; rclcpp::Time current_time = this->clock_->now(); - tf2::Transform tf_base_link2front( - tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(base_link2front_, 0.0, 0.0)); // visualize stop line if (stop_pose_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "virtual_wall/no_start"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::CUBE; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 1.0; - marker.scale.x = 0.1; - marker.scale.y = 5.0; - marker.scale.z = 2.0; - marker.color.a = 0.5; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - msg.markers.push_back(marker); - } - - // visualize stop reason - if (stop_pose_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "factor_text/no_start"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; - tf2::Transform tf_map2base_link; - tf2::fromMsg(*stop_pose_ptr_, tf_map2base_link); - tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; - tf2::toMsg(tf_map2front, marker.pose); - marker.pose.position.z += 2.0; - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 1.0; - marker.color.a = 0.999; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; - marker.text = "surround obstacle"; - msg.markers.push_back(marker); + const auto p = calcOffsetPose(*stop_pose_ptr_, base_link2front_, 0.0, 0.0); + const auto markers = createStopVirtualWallMarker(p, "surround obstacle", current_time, 0); + appendMarkerArray(markers, &msg); } // visualize surround object if (stop_obstacle_point_ptr_ != nullptr) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = current_time; - marker.ns = "no_start_obstacle_text"; - marker.id = 0; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; + auto marker = createDefaultMarker( + "map", current_time, "no_start_obstacle_text", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); marker.pose.position = *stop_obstacle_point_ptr_; marker.pose.position.z += 2.0; // add half of the heights of obj roughly - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.0; - marker.scale.y = 0.0; - marker.scale.z = 1.0; - marker.color.a = 0.999; // Don't forget to set the alpha! - marker.color.r = 1.0; - marker.color.g = 1.0; - marker.color.b = 1.0; marker.text = "!"; msg.markers.push_back(marker); } @@ -158,7 +107,7 @@ visualization_msgs::msg::MarkerArray SurroundObstacleCheckerDebugNode::makeVisua return msg; } -tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() +StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() { // create header std_msgs::msg::Header header; @@ -166,9 +115,9 @@ tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::make header.stamp = this->clock_->now(); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::SURROUND_OBSTACLE_CHECK; - tier4_planning_msgs::msg::StopFactor stop_factor; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::SURROUND_OBSTACLE_CHECK; + StopFactor stop_factor; if (stop_pose_ptr_ != nullptr) { stop_factor.stop_pose = *stop_pose_ptr_; @@ -179,8 +128,10 @@ tier4_planning_msgs::msg::StopReasonArray SurroundObstacleCheckerDebugNode::make } // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; } + +} // namespace surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 09fe48f35b57..9e1fc6711bd3 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,9 +14,17 @@ #include "surround_obstacle_checker/node.hpp" +#include + +#include +#include +#include +#include +#include +#include + #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -34,313 +42,381 @@ #include #include +namespace surround_obstacle_checker +{ + +namespace bg = boost::geometry; +using Point2d = bg::model::d2::point_xy; +using Polygon2d = bg::model::polygon; +using tier4_autoware_utils::createPoint; +using tier4_autoware_utils::pose2transform; + +namespace +{ +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; + no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + no_start_reason_diag.name = "no_start_reason"; + no_start_reason_diag.message = no_start_reason; + no_start_reason_diag_kv.key = "no_start_pose"; + no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); + no_start_reason_diag.values.push_back(no_start_reason_diag_kv); + return no_start_reason_diag; +} + +geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) +{ + geometry_msgs::msg::Point32 p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) +{ + geometry_msgs::msg::Polygon transformed_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(pose); + tf2::doTransform(footprint, transformed_polygon, geometry_tf); + + Polygon2d object_polygon; + for (const auto & p : transformed_polygon.points) { + object_polygon.outer().push_back(Point2d(p.x, p.y)); + } + + bg::correct(object_polygon); + + return object_polygon; +} + +Polygon2d createObjPolygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +{ + const double & length_m = size.x / 2.0; + const double & width_m = size.y / 2.0; + + geometry_msgs::msg::Polygon polygon{}; + + polygon.points.push_back(createPoint32(length_m, -width_m, 0.0)); + polygon.points.push_back(createPoint32(length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, width_m, 0.0)); + polygon.points.push_back(createPoint32(-length_m, -width_m, 0.0)); + + return createObjPolygon(pose, polygon); +} + +Polygon2d createSelfPolygon(const VehicleInfo & vehicle_info) +{ + const double & front_m = vehicle_info.max_longitudinal_offset_m; + const double & width_m = vehicle_info.min_lateral_offset_m; + const double & rear_m = vehicle_info.min_longitudinal_offset_m; + + Polygon2d ego_polygon; + + ego_polygon.outer().push_back(Point2d(front_m, -width_m)); + ego_polygon.outer().push_back(Point2d(front_m, width_m)); + ego_polygon.outer().push_back(Point2d(-rear_m, width_m)); + ego_polygon.outer().push_back(Point2d(-rear_m, -width_m)); + + bg::correct(ego_polygon); + + return ego_polygon; +} +} // namespace + SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) -: Node("surround_obstacle_checker_node", node_options), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) +: Node("surround_obstacle_checker_node", node_options) { // Parameters - use_pointcloud_ = this->declare_parameter("use_pointcloud", true); - use_dynamic_object_ = this->declare_parameter("use_dynamic_object", true); - surround_check_distance_ = this->declare_parameter("surround_check_distance", 2.0); - surround_check_recover_distance_ = - this->declare_parameter("surround_check_recover_distance", 2.5); - state_clear_time_ = this->declare_parameter("state_clear_time", 2.0); - stop_state_ego_speed_ = this->declare_parameter("stop_state_ego_speed", 0.1); - debug_ptr_ = std::make_shared( - vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this); - self_poly_ = createSelfPolygon(); + { + auto & p = node_param_; + p.use_pointcloud = this->declare_parameter("use_pointcloud", true); + p.use_dynamic_object = this->declare_parameter("use_dynamic_object", true); + p.surround_check_distance = this->declare_parameter("surround_check_distance", 2.0); + p.surround_check_recover_distance = + this->declare_parameter("surround_check_recover_distance", 2.5); + p.state_clear_time = this->declare_parameter("state_clear_time", 2.0); + p.stop_state_ego_speed = this->declare_parameter("stop_state_ego_speed", 0.1); + p.stop_state_entry_duration_time = + this->declare_parameter("stop_state_entry_duration_time", 0.1); + } + + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); // Publishers - path_pub_ = - this->create_publisher("~/output/trajectory", 1); - stop_reason_diag_pub_ = + pub_stop_reason_ = this->create_publisher("~/output/no_start_reason", 1); + pub_clear_velocity_limit_ = + this->create_publisher("~/output/velocity_limit_clear_command", 1); + pub_velocity_limit_ = this->create_publisher("~/output/max_velocity", 1); - // Subscriber - path_sub_ = this->create_subscription( - "~/input/trajectory", 1, - std::bind(&SurroundObstacleCheckerNode::pathCallback, this, std::placeholders::_1)); - pointcloud_sub_ = this->create_subscription( + // Subscribers + sub_pointcloud_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&SurroundObstacleCheckerNode::pointCloudCallback, this, std::placeholders::_1)); - dynamic_object_sub_ = - this->create_subscription( - "~/input/objects", 1, - std::bind(&SurroundObstacleCheckerNode::dynamicObjectCallback, this, std::placeholders::_1)); - current_velocity_sub_ = this->create_subscription( + std::bind(&SurroundObstacleCheckerNode::onPointCloud, this, std::placeholders::_1)); + sub_dynamic_objects_ = this->create_subscription( + "~/input/objects", 1, + std::bind(&SurroundObstacleCheckerNode::onDynamicObjects, this, std::placeholders::_1)); + sub_odometry_ = this->create_subscription( "~/input/odometry", 1, - std::bind(&SurroundObstacleCheckerNode::currentVelocityCallback, this, std::placeholders::_1)); + std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); + + using std::chrono_literals::operator""ms; + timer_ = rclcpp::create_timer( + this, get_clock(), 100ms, std::bind(&SurroundObstacleCheckerNode::onTimer, this)); + + last_running_time_ = std::make_shared(this->now()); + + // Debug + debug_ptr_ = std::make_shared( + vehicle_info_.max_longitudinal_offset_m, this->get_clock(), *this); } -void SurroundObstacleCheckerNode::pathCallback( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onTimer() { - if (use_pointcloud_ && !pointcloud_ptr_) { + if (node_param_.use_pointcloud && !pointcloud_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for pointcloud info..."); return; } - if (use_dynamic_object_ && !object_ptr_) { + if (node_param_.use_dynamic_object && !object_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for dynamic object info..."); return; } - if (!current_velocity_ptr_) { + if (!odometry_ptr_) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 1000 /* ms */, "waiting for current velocity..."); return; } - // parameter description - TrajectoryPoints output_trajectory_points = - tier4_autoware_utils::convertToTrajectoryPointArray(*input_msg); + const auto nearest_obstacle = getNearestObstacle(); + const auto is_vehicle_stopped = isVehicleStopped(); - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + switch (state_) { + case State::PASS: { + const auto is_obstacle_found = + !nearest_obstacle ? false + : nearest_obstacle.get().first < node_param_.surround_check_distance; - // get current pose in traj frame - geometry_msgs::msg::Pose current_pose; - if (!getPose(input_msg->header.frame_id, "base_link", current_pose)) { - return; - } + if (!isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + break; + } + + state_ = State::STOP; - // get closest idx - const size_t closest_idx = getClosestIdx(output_trajectory_points, current_pose); + auto velocity_limit = std::make_shared(); + velocity_limit->stamp = this->now(); + velocity_limit->max_velocity = 0.0; + velocity_limit->use_constraints = false; + velocity_limit->sender = "surround_obstacle_checker"; - // get nearest object - double min_dist_to_obj = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_obj_point; - getNearestObstacle(&min_dist_to_obj, &nearest_obj_point); + pub_velocity_limit_->publish(*velocity_limit); - // check current obstacle status (exist or not) - const auto is_obstacle_found = isObstacleFound(min_dist_to_obj); + // do not start when there is a obstacle near the ego vehicle. + RCLCPP_WARN(get_logger(), "do not start because there is obstacle near the ego vehicle."); - // check current stop status (stop or not) - const auto is_stopped = checkStop(input_msg->points.at(closest_idx)); + break; + } - const auto is_stop_required = isStopRequired(is_obstacle_found, is_stopped); + case State::STOP: { + const auto is_obstacle_found = + !nearest_obstacle + ? false + : nearest_obstacle.get().first < node_param_.surround_check_recover_distance; - // insert stop velocity - if (is_stop_required) { - state_ = State::STOP; + if (isStopRequired(is_obstacle_found, is_vehicle_stopped)) { + break; + } - // do not start when there is a obstacle near the ego vehicle. - RCLCPP_WARN_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "do not start because there is obstacle near the ego vehicle."); - insertStopVelocity(closest_idx, &output_trajectory_points); + state_ = State::PASS; + + auto velocity_limit_clear_command = std::make_shared(); + velocity_limit_clear_command->stamp = this->now(); + velocity_limit_clear_command->command = true; + velocity_limit_clear_command->sender = "surround_obstacle_checker"; - // visualization for debug - if (is_obstacle_found) { - debug_ptr_->pushObstaclePoint(nearest_obj_point, PointType::NoStart); + pub_clear_velocity_limit_->publish(*velocity_limit_clear_command); + + break; } - debug_ptr_->pushPose(input_msg->points.at(closest_idx).pose, PoseType::NoStart); - no_start_reason_diag = makeStopReasonDiag("obstacle", input_msg->points.at(closest_idx).pose); - } else { - state_ = State::PASS; + + default: + break; } - // publish trajectory and debug info - auto output_msg = tier4_autoware_utils::convertToTrajectory(output_trajectory_points); - output_msg.header = input_msg->header; - path_pub_->publish(output_msg); - stop_reason_diag_pub_->publish(no_start_reason_diag); + if (nearest_obstacle) { + debug_ptr_->pushObstaclePoint(nearest_obstacle.get().second, PointType::NoStart); + } + + diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; + if (state_ == State::STOP) { + debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); + no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); + } + + pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } -void SurroundObstacleCheckerNode::pointCloudCallback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { - pointcloud_ptr_ = input_msg; + pointcloud_ptr_ = msg; } -void SurroundObstacleCheckerNode::dynamicObjectCallback( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) { - object_ptr_ = input_msg; + object_ptr_ = msg; } -void SurroundObstacleCheckerNode::currentVelocityCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr input_msg) +void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) { - current_velocity_ptr_ = input_msg; + odometry_ptr_ = msg; } -void SurroundObstacleCheckerNode::insertStopVelocity( - const size_t closest_idx, TrajectoryPoints * traj) +boost::optional SurroundObstacleCheckerNode::getNearestObstacle() const { - // set zero velocity from closest idx to last idx - for (size_t i = closest_idx; i < traj->size(); i++) { - traj->at(i).longitudinal_velocity_mps = 0.0; - } -} + boost::optional nearest_pointcloud{boost::none}; + boost::optional nearest_object{boost::none}; -bool SurroundObstacleCheckerNode::getPose( - const std::string & source, const std::string & target, geometry_msgs::msg::Pose & pose) -{ - try { - // get transform from source to target - geometry_msgs::msg::TransformStamped ros_src2tgt = - tf_buffer_.lookupTransform(source, target, tf2::TimePointZero); - // convert geometry_msgs::msg::Transform to geometry_msgs::msg::Pose - tf2::Transform transform; - tf2::fromMsg(ros_src2tgt.transform, transform); - tf2::toMsg(transform, pose); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "cannot get tf from " << source << " to " << target); - return false; + if (node_param_.use_pointcloud) { + nearest_pointcloud = getNearestObstacleByPointCloud(); } - return true; -} -bool SurroundObstacleCheckerNode::convertPose( - const geometry_msgs::msg::Pose & pose, const std::string & source, const std::string & target, - const rclcpp::Time & time, geometry_msgs::msg::Pose & conv_pose) -{ - tf2::Transform src2tgt; - try { - // get transform from source to target - geometry_msgs::msg::TransformStamped ros_src2tgt = - tf_buffer_.lookupTransform(source, target, time, tf2::durationFromSec(0.1)); - tf2::fromMsg(ros_src2tgt.transform, src2tgt); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "cannot get tf from " << source << " to " << target); - return false; + if (node_param_.use_dynamic_object) { + nearest_object = getNearestObstacleByDynamicObject(); } - tf2::Transform src2obj; - tf2::fromMsg(pose, src2obj); - tf2::Transform tgt2obj = src2tgt.inverse() * src2obj; - tf2::toMsg(tgt2obj, conv_pose); - return true; -} - -size_t SurroundObstacleCheckerNode::getClosestIdx( - const TrajectoryPoints & traj, const geometry_msgs::msg::Pose current_pose) -{ - double min_dist = std::numeric_limits::max(); - size_t min_dist_idx = 0; - for (size_t i = 0; i < traj.size(); ++i) { - const double x = traj.at(i).pose.position.x - current_pose.position.x; - const double y = traj.at(i).pose.position.y - current_pose.position.y; - const double dist = std::hypot(x, y); - if (dist < min_dist) { - min_dist_idx = i; - min_dist = dist; - } + if (!nearest_pointcloud && !nearest_object) { + return {}; } - return min_dist_idx; -} -void SurroundObstacleCheckerNode::getNearestObstacle( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) -{ - if (use_pointcloud_) { - getNearestObstacleByPointCloud(min_dist_to_obj, nearest_obj_point); + if (!nearest_pointcloud) { + return nearest_object; } - if (use_dynamic_object_) { - getNearestObstacleByDynamicObject(min_dist_to_obj, nearest_obj_point); + if (!nearest_object) { + return nearest_pointcloud; } + + return nearest_pointcloud.get().first < nearest_object.get().first ? nearest_pointcloud + : nearest_object; } -void SurroundObstacleCheckerNode::getNearestObstacleByPointCloud( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +boost::optional SurroundObstacleCheckerNode::getNearestObstacleByPointCloud() const { - // wait to transform pointcloud - geometry_msgs::msg::TransformStamped transform_stamped; - try { - transform_stamped = tf_buffer_.lookupTransform( - "base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, - tf2::durationFromSec(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *this->get_clock(), 500 /* ms */, - "failed to get base_link to " << pointcloud_ptr_->header.frame_id << " transform."); - return; + const auto transform_stamped = + getTransform("base_link", pointcloud_ptr_->header.frame_id, pointcloud_ptr_->header.stamp, 0.5); + + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; } - Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.transform).cast(); - pcl::PointCloud pcl; - pcl::fromROSMsg(*pointcloud_ptr_, pcl); - pcl::transformPointCloud(pcl, pcl, isometry); - for (const auto & p : pcl) { - // create boost point - Point2d boost_p(p.x, p.y); - - // calc distance - const double dist_to_obj = boost::geometry::distance(self_poly_, boost_p); - - // get minimum distance to obj - if (dist_to_obj < *min_dist_to_obj) { - *min_dist_to_obj = dist_to_obj; - nearest_obj_point->x = p.x; - nearest_obj_point->y = p.y; - nearest_obj_point->z = p.z; + Eigen::Affine3f isometry = tf2::transformToEigen(transform_stamped.get().transform).cast(); + pcl::PointCloud transformed_pointcloud; + pcl::fromROSMsg(*pointcloud_ptr_, transformed_pointcloud); + pcl::transformPointCloud(transformed_pointcloud, transformed_pointcloud, isometry); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); + + for (const auto & p : transformed_pointcloud) { + Point2d boost_point(p.x, p.y); + + const auto distance_to_object = bg::distance(ego_polygon, boost_point); + + if (distance_to_object < minimum_distance) { + nearest_point = createPoint(p.x, p.y, p.z); + minimum_distance = distance_to_object; } } + + return std::make_pair(minimum_distance, nearest_point); } -void SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject( - double * min_dist_to_obj, geometry_msgs::msg::Point * nearest_obj_point) +boost::optional SurroundObstacleCheckerNode::getNearestObstacleByDynamicObject() const { - const auto obj_frame = object_ptr_->header.frame_id; - const auto obj_time = object_ptr_->header.stamp; - for (const auto & obj : object_ptr_->objects) { - // change frame of obj_pose to base_link - geometry_msgs::msg::Pose pose_baselink; - if (!convertPose( - obj.kinematics.initial_pose_with_covariance.pose, obj_frame, "base_link", obj_time, - pose_baselink)) { - return; - } + const auto transform_stamped = + getTransform(object_ptr_->header.frame_id, "base_link", object_ptr_->header.stamp, 0.5); - // create obj polygon - Polygon2d obj_poly; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { - obj_poly = createObjPolygon(pose_baselink, obj.shape.footprint); - } else { - obj_poly = createObjPolygon(pose_baselink, obj.shape.dimensions); - } + geometry_msgs::msg::Point nearest_point; + auto minimum_distance = std::numeric_limits::max(); + + if (!transform_stamped) { + return {}; + } + + tf2::Transform tf_src2target; + tf2::fromMsg(transform_stamped.get().transform, tf_src2target); + + const auto ego_polygon = createSelfPolygon(vehicle_info_); - // calc distance - const double dist_to_obj = boost::geometry::distance(self_poly_, obj_poly); + for (const auto & object : object_ptr_->objects) { + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // get minimum distance to obj - if (dist_to_obj < *min_dist_to_obj) { - *min_dist_to_obj = dist_to_obj; - *nearest_obj_point = obj.kinematics.initial_pose_with_covariance.pose.position; + tf2::Transform tf_src2object; + tf2::fromMsg(object_pose, tf_src2object); + + geometry_msgs::msg::Pose transformed_object_pose; + tf2::toMsg(tf_src2target.inverse() * tf_src2object, transformed_object_pose); + + const auto object_polygon = + object.shape.type == Shape::POLYGON + ? createObjPolygon(transformed_object_pose, object.shape.footprint) + : createObjPolygon(transformed_object_pose, object.shape.dimensions); + + const auto distance_to_object = bg::distance(ego_polygon, object_polygon); + + if (distance_to_object < minimum_distance) { + nearest_point = object_pose.position; + minimum_distance = distance_to_object; } } + + return std::make_pair(minimum_distance, nearest_point); } -bool SurroundObstacleCheckerNode::isObstacleFound(const double min_dist_to_obj) +boost::optional SurroundObstacleCheckerNode::getTransform( + const std::string & source, const std::string & target, const rclcpp::Time & stamp, + double duration_sec) const { - const auto is_obstacle_inside_range = min_dist_to_obj < surround_check_distance_; - const auto is_obstacle_outside_range = min_dist_to_obj > surround_check_recover_distance_; - - if (state_ == State::PASS) { - return is_obstacle_inside_range; - } + geometry_msgs::msg::TransformStamped transform_stamped; - if (state_ == State::STOP) { - return !is_obstacle_outside_range; + try { + transform_stamped = + tf_buffer_.lookupTransform(source, target, stamp, tf2::durationFromSec(duration_sec)); + } catch (tf2::TransformException & ex) { + return {}; } - throw std::runtime_error("invalid state"); + return transform_stamped; } bool SurroundObstacleCheckerNode::isStopRequired( - const bool is_obstacle_found, const bool is_stopped) + const bool is_obstacle_found, const bool is_vehicle_stopped) { - if (!is_stopped) { + if (!is_vehicle_stopped) { return false; } @@ -356,7 +432,7 @@ bool SurroundObstacleCheckerNode::isStopRequired( // Keep stop state if (last_obstacle_found_time_) { const auto elapsed_time = this->now() - *last_obstacle_found_time_; - if (elapsed_time.seconds() <= state_clear_time_) { + if (elapsed_time.seconds() <= node_param_.state_clear_time) { return true; } } @@ -365,118 +441,18 @@ bool SurroundObstacleCheckerNode::isStopRequired( return false; } -bool SurroundObstacleCheckerNode::checkStop( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & closest_point) -{ - if (std::fabs(current_velocity_ptr_->twist.twist.linear.x) > stop_state_ego_speed_) { - // ego vehicle has high velocity now. not stop. - return false; - } - - const double closest_vel = closest_point.longitudinal_velocity_mps; - const double closest_acc = closest_point.acceleration_mps2; - - if (closest_vel * closest_acc < 0) { - // ego vehicle is about to stop (during deceleration). not stop. - return false; - } - - return true; -} - -Polygon2d SurroundObstacleCheckerNode::createSelfPolygon() -{ - const double front = vehicle_info_.max_longitudinal_offset_m; - const double rear = vehicle_info_.min_longitudinal_offset_m; - const double left = vehicle_info_.max_lateral_offset_m; - const double right = vehicle_info_.min_lateral_offset_m; - - Polygon2d poly; - boost::geometry::exterior_ring(poly) = boost::assign::list_of(front, left)(front, right)( - rear, right)(rear, left)(front, left); - return poly; -} - -Polygon2d SurroundObstacleCheckerNode::createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & size) +bool SurroundObstacleCheckerNode::isVehicleStopped() { - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double h = size.x; - const double w = size.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( - -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); - - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} + const auto current_velocity = std::abs(odometry_ptr_->twist.twist.linear.x); -Polygon2d SurroundObstacleCheckerNode::createObjPolygon( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Polygon & footprint) -{ - // rename - const double x = pose.position.x; - const double y = pose.position.y; - const double yaw = tf2::getYaw(pose.orientation); - - // create base polygon - Polygon2d obj_poly; - for (const auto point : footprint.points) { - const Point2d point2d(point.x, point.y); - obj_poly.outer().push_back(point2d); + if (node_param_.stop_state_ego_speed < current_velocity) { + last_running_time_ = std::make_shared(this->now()); } - // rotate polygon(yaw) - boost::geometry::strategy::transform::rotate_transformer - rotate(-yaw); // anti-clockwise -> :clockwise rotation - Polygon2d rotate_obj_poly; - boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); - - // translate polygon(x, y) - boost::geometry::strategy::transform::translate_transformer translate(x, y); - Polygon2d translate_obj_poly; - boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); - return translate_obj_poly; -} - -diagnostic_msgs::msg::DiagnosticStatus SurroundObstacleCheckerNode::makeStopReasonDiag( - const std::string no_start_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; - diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; - no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - no_start_reason_diag.name = "no_start_reason"; - no_start_reason_diag.message = no_start_reason; - no_start_reason_diag_kv.key = "no_start_pose"; - no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); - no_start_reason_diag.values.push_back(no_start_reason_diag_kv); - return no_start_reason_diag; + return node_param_.stop_state_entry_duration_time < (this->now() - *last_running_time_).seconds(); } -std::string SurroundObstacleCheckerNode::jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} +} // namespace surround_obstacle_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(SurroundObstacleCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(surround_obstacle_checker::SurroundObstacleCheckerNode)