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)