From e5c859149955d1ff71a31ed8fb04629760af0a2c Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Fri, 30 Oct 2020 18:06:47 +0900 Subject: [PATCH] Port lane change planner (#60) * port CMakeLists.txt Signed-off-by: mitsudome-r * port headers Signed-off-by: mitsudome-r * port loggers Signed-off-by: mitsudome-r * port time, duration, clock Signed-off-by: mitsudome-r * fix warnings Signed-off-by: mitsudome-r * port publishers and subscribers Signed-off-by: mitsudome-r * port parameters Signed-off-by: mitsudome-r * port tf Signed-off-by: mitsudome-r * other Signed-off-by: mitsudome-r --- .../lane_change_planner/CMakeLists.txt | 38 +-- .../lane_change_planner/COLCON_IGNORE | 0 .../lane_change_planner/data_manager.h | 55 ++-- .../lane_change_planner/lane_changer.h | 59 ++-- .../lane_change_planner/route_handler.h | 53 +-- .../state/aborting_lane_change.h | 2 +- .../state/blocked_by_obstacle.h | 20 +- .../state/common_functions.h | 28 +- .../state/executing_lane_change.h | 16 +- .../state/following_lane.h | 14 +- .../state/forcing_lane_change.h | 14 +- .../state/state_base_class.h | 14 +- .../state/stopping_lane_change.h | 12 +- .../lane_change_planner/state_machine.h | 11 +- .../include/lane_change_planner/utilities.h | 126 ++++---- .../lane_change_planner/package.xml | 9 +- .../lane_change_planner/src/data_manager.cpp | 62 ++-- .../src/lane_change_planner_node.cpp | 8 +- .../lane_change_planner/src/lane_changer.cpp | 301 +++++++++--------- .../lane_change_planner/src/route_handler.cpp | 81 ++--- .../src/state/aborting_lane_change.cpp | 2 +- .../src/state/blocked_by_obstacle.cpp | 33 +- .../src/state/common_functions.cpp | 45 ++- .../src/state/executing_lane_change.cpp | 19 +- .../src/state/following_lane.cpp | 20 +- .../src/state/forcing_lane_change.cpp | 9 +- .../src/state/state_base_class.cpp | 2 +- .../src/state/stopping_lane_change.cpp | 12 +- .../lane_change_planner/src/state_machine.cpp | 12 +- .../lane_change_planner/src/utilities.cpp | 294 +++++++++-------- 30 files changed, 708 insertions(+), 663 deletions(-) delete mode 100644 planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/COLCON_IGNORE diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt index c64a4bd8b0993..604674ee9513e 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/CMakeLists.txt @@ -1,31 +1,24 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(lane_change_planner) -add_compile_options(-std=c++14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() -find_package(catkin REQUIRED COMPONENTS - autoware_perception_msgs - autoware_planning_msgs - geometry_msgs - lanelet2_extension - roscpp - sensor_msgs - tf2 - tf2_geometry_msgs - tf2_ros -) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -catkin_package() - include_directories( - include - ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ) -add_executable(lane_change_planner +ament_auto_add_executable(lane_change_planner src/lane_change_planner_node.cpp src/lane_changer.cpp src/data_manager.cpp @@ -42,16 +35,9 @@ add_executable(lane_change_planner src/state/stopping_lane_change.cpp src/state/common_functions.cpp ) -add_dependencies(lane_change_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(lane_change_planner - ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ) -install( - TARGETS - lane_change_planner - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +ament_auto_package( ) diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/COLCON_IGNORE b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h index 7676df422ee46..ccba2e26b6de7 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/data_manager.h @@ -18,18 +18,18 @@ #define LANE_CHANGE_PLANNER_DATA_MANAGER_H // ROS -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include // Autoware -#include -#include -#include -#include +#include +#include +#include +#include #include // lanelet @@ -45,20 +45,22 @@ namespace lane_change_planner class SelfPoseLinstener { public: - SelfPoseLinstener(); - bool getSelfPose(geometry_msgs::PoseStamped & self_pose); + SelfPoseLinstener(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); + bool getSelfPose(geometry_msgs::msg::PoseStamped & self_pose); bool isSelfPoseReady(); private: - tf2_ros::Buffer tf_buffer_; + tf2::BufferCore tf_buffer_; tf2_ros::TransformListener tf_listener_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; }; struct BoolStamped { explicit BoolStamped(bool in_data) : data(in_data) {} bool data = false; - ros::Time stamp; + rclcpp::Time stamp; }; class DataManager @@ -67,40 +69,45 @@ class DataManager /* * Cache */ - autoware_perception_msgs::DynamicObjectArray::ConstPtr perception_ptr_; - geometry_msgs::TwistStamped::ConstPtr vehicle_velocity_ptr_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr perception_ptr_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr vehicle_velocity_ptr_; BoolStamped lane_change_approval_; BoolStamped force_lane_change_; - geometry_msgs::PoseStamped self_pose_; + geometry_msgs::msg::PoseStamped self_pose_; // ROS parameters LaneChangerParameters parameters_; bool is_parameter_set_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + /* * SelfPoseLinstener */ std::shared_ptr self_pose_listener_ptr_; public: - DataManager(); + DataManager(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); ~DataManager() = default; // callbacks void perceptionCallback( - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & input_perception_msg); - void velocityCallback(const geometry_msgs::TwistStamped::ConstPtr & input_twist_msg); + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_perception_msg); + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_twist_msg); void setLaneChangerParameters(const LaneChangerParameters & parameters); - void laneChangeApprovalCallback(const std_msgs::Bool & input_approval_msg); - void forceLaneChangeSignalCallback(const std_msgs::Bool & input_approval_msg); + void laneChangeApprovalCallback(const std_msgs::msg::Bool::ConstSharedPtr input_approval_msg); + void forceLaneChangeSignalCallback(const std_msgs::msg::Bool::ConstSharedPtr input_approval_msg); // getters - autoware_perception_msgs::DynamicObjectArray::ConstPtr getDynamicObjects(); - geometry_msgs::PoseStamped getCurrentSelfPose(); - geometry_msgs::TwistStamped::ConstPtr getCurrentSelfVelocity(); + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr getDynamicObjects(); + geometry_msgs::msg::PoseStamped getCurrentSelfPose(); + geometry_msgs::msg::TwistStamped::ConstSharedPtr getCurrentSelfVelocity(); LaneChangerParameters getLaneChangerParameters(); bool getLaneChangeApproval(); bool getForceLaneChangeSignal(); + rclcpp::Logger & getLogger(); + rclcpp::Clock::SharedPtr getClock(); bool isDataReady(); }; diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h index 7fbbfb1eecd74..0efdea4e2e0a0 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/lane_changer.h @@ -18,16 +18,18 @@ #define LANE_CHANGE_PLANNER_LANE_CHANGER_H // ROS -#include -#include +#include +#include #include +#include // Autoware -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -42,42 +44,39 @@ namespace lane_change_planner { -class LaneChanger +class LaneChanger : public rclcpp::Node { private: - ros::Timer timer_; + rclcpp::TimerBase::SharedPtr timer_; - ros::Publisher path_publisher_; - ros::Publisher candidate_path_publisher_; - ros::Publisher path_marker_publisher_; - ros::Publisher stop_reason_publisher_; - ros::Publisher drivable_area_publisher_; - ros::Publisher lane_change_ready_publisher_; - ros::Publisher lane_change_available_publisher_; + rclcpp::Publisher::SharedPtr path_publisher_; + rclcpp::Publisher::SharedPtr candidate_path_publisher_; + rclcpp::Publisher::SharedPtr path_marker_publisher_; + rclcpp::Publisher::SharedPtr stop_reason_publisher_; + rclcpp::Publisher::SharedPtr drivable_area_publisher_; + rclcpp::Publisher::SharedPtr lane_change_ready_publisher_; + rclcpp::Publisher::SharedPtr lane_change_available_publisher_; - ros::NodeHandle pnh_; + rclcpp::Subscription::SharedPtr perception_subscriber_; + rclcpp::Subscription::SharedPtr velocity_subscriber_; + rclcpp::Subscription::SharedPtr lane_change_approval_subscriber_; + rclcpp::Subscription::SharedPtr force_lane_change_subscriber_; - ros::Subscriber points_subscriber_; - ros::Subscriber perception_subscriber_; - ros::Subscriber velocity_subscriber_; - ros::Subscriber lane_change_approval_subscriber_; - ros::Subscriber force_lane_change_subscriber_; - - ros::Subscriber vector_map_subscriber_; - ros::Subscriber route_subscriber_; - ros::Subscriber route_init_subscriber_; + rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr route_subscriber_; + rclcpp::Subscription::SharedPtr route_init_subscriber_; std::shared_ptr data_manager_ptr_; std::shared_ptr state_machine_ptr_; std::shared_ptr route_handler_ptr_; // PathExtender path_extender_; - void run(const ros::TimerEvent & event); + void run(); void publishDebugMarkers(); - void publishDrivableArea(const autoware_planning_msgs::PathWithLaneId & path); - autoware_planning_msgs::StopReasonArray makeStopReasonArray( + void publishDrivableArea(const autoware_planning_msgs::msg::PathWithLaneId & path); + autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray( const DebugData & debug_data, const State & state); - std::vector makeEmptyStopReasons(); + std::vector makeEmptyStopReasons(); void waitForData(); public: diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/route_handler.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/route_handler.h index cbcd38844bba8..f89fd2ba4f831 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/route_handler.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/route_handler.h @@ -18,10 +18,10 @@ #define LANE_CHANGE_PLANNER_ROUTE_HANDLER_H // Autoware -#include -#include -#include -#include +#include +#include +#include +#include #include // lanelet #include @@ -32,6 +32,8 @@ #include +#include + #include namespace lane_change_planner @@ -40,7 +42,7 @@ enum class LaneChangeDirection { NONE, LEFT, RIGHT }; struct LaneChangePath { - autoware_planning_msgs::PathWithLaneId path; + autoware_planning_msgs::msg::PathWithLaneId path; double acceleration = 0.0; double preparation_length = 0.0; double lane_change_length = 0.0; @@ -49,7 +51,7 @@ struct LaneChangePath class RouteHandler { public: - RouteHandler(); + RouteHandler(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); ~RouteHandler() = default; private: @@ -57,7 +59,7 @@ class RouteHandler lanelet::routing::RoutingGraphPtr routing_graph_ptr_; lanelet::traffic_rules::TrafficRulesPtr traffic_rules_ptr_; std::shared_ptr overall_graphs_ptr_; - autoware_planning_msgs::Route route_msg_; + autoware_planning_msgs::msg::Route route_msg_; bool is_map_msg_ready_; bool is_route_msg_ready_; @@ -68,6 +70,9 @@ class RouteHandler lanelet::ConstLanelets start_lanelets_; lanelet::ConstLanelets goal_lanelets_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + void setRouteLanelets(); friend class LaneChanger; @@ -78,8 +83,8 @@ class RouteHandler const lanelet::routing::RoutingGraphPtr & routing_graph, const lanelet::routing::Route & route); bool isHandlerReady(); - void mapCallback(const autoware_lanelet2_msgs::MapBin & map_msg); - void routeCallback(const autoware_planning_msgs::Route & route_msg); + void mapCallback(const autoware_lanelet2_msgs::msg::MapBin::ConstSharedPtr map_msg); + void routeCallback(const autoware_planning_msgs::msg::Route::ConstSharedPtr route_msg); bool getPreviousLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; @@ -91,20 +96,20 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet); bool getClosestLaneletWithinRoute( - const geometry_msgs::Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + const geometry_msgs::msg::Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; - geometry_msgs::Pose getGoalPose() const; + geometry_msgs::msg::Pose getGoalPose() const; lanelet::Id getGoalLaneId() const; lanelet::ConstLanelets getLaneletsFromIds(const std::vector ids) const; bool isDeadEndLanelet(const lanelet::ConstLanelet & lanelet) const; bool isInTargetLane( - const geometry_msgs::PoseStamped & pose, const lanelet::ConstLanelets & target) const; + const geometry_msgs::msg::PoseStamped & pose, const lanelet::ConstLanelets & target) const; bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getRouteLanelets() const; lanelet::ConstLanelets getLaneletSequence(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneletSequence( - const lanelet::ConstLanelet & lanelet, const geometry_msgs::Pose & current_pose, + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & current_pose, const double backward_distance, const double forward_distance) const; lanelet::ConstLanelets getLaneletSequenceUpTo( const lanelet::ConstLanelet & lanelet, @@ -114,33 +119,33 @@ class RouteHandler const double min_length = std::numeric_limits::max()) const; lanelet::ConstLanelets getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const; - lanelet::ConstLanelets getClosestLaneletSequence(const geometry_msgs::Pose & pose) const; + lanelet::ConstLanelets getClosestLaneletSequence(const geometry_msgs::msg::Pose & pose) const; lanelet::ConstLanelets getNeighborsWithinRoute(const lanelet::ConstLanelet & lanelet) const; int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; - bool isInPreferredLane(const geometry_msgs::PoseStamped & pose) const; + bool isInPreferredLane(const geometry_msgs::msg::PoseStamped & pose) const; std::vector getLaneChangePaths( const lanelet::ConstLanelets & original_lanes, const lanelet::ConstLanelets & target_lanes, - const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist, const LaneChangerParameters & parameter) const; - autoware_planning_msgs::PathWithLaneId getReferencePath( - const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::Pose & pose, + autoware_planning_msgs::msg::PathWithLaneId getReferencePath( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose, const double backward_path_length, const double forward_path_length, const double minimum_lane_change_length) const; - autoware_planning_msgs::PathWithLaneId getReferencePath( + autoware_planning_msgs::msg::PathWithLaneId getReferencePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact = true) const; - autoware_planning_msgs::PathWithLaneId updatePathTwist( - const autoware_planning_msgs::PathWithLaneId & path) const; + autoware_planning_msgs::msg::PathWithLaneId updatePathTwist( + const autoware_planning_msgs::msg::PathWithLaneId & path) const; bool getLaneChangeTarget( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * target_lanelet) const; - lanelet::ConstLanelets getLaneChangeTarget(const geometry_msgs::Pose & pose) const; + lanelet::ConstLanelets getLaneChangeTarget(const geometry_msgs::msg::Pose & pose) const; double getLaneChangeableDistance( - const geometry_msgs::Pose & current_pose, const LaneChangeDirection & direction); + const geometry_msgs::msg::Pose & current_pose, const LaneChangeDirection & direction); lanelet::ConstLanelets getCheckTargetLanesFromPath( - const autoware_planning_msgs::PathWithLaneId & path, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, const double check_length); lanelet::routing::RoutingGraphContainer getOverallGraph() const; diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/aborting_lane_change.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/aborting_lane_change.h index 000268a231e18..eefe41f9738ef 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/aborting_lane_change.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/aborting_lane_change.h @@ -37,7 +37,7 @@ class AbortingLaneChangeState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/blocked_by_obstacle.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/blocked_by_obstacle.h index 85624d533b871..627ea038ddd33 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/blocked_by_obstacle.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/blocked_by_obstacle.h @@ -17,9 +17,9 @@ #ifndef LANE_CHANGE_PLANNER_STATE_BLOCKED_BY_OBSTACLE_H #define LANE_CHANGE_PLANNER_STATE_BLOCKED_BY_OBSTACLE_H -#include -#include -#include +#include +#include +#include #include #include #include @@ -29,9 +29,9 @@ namespace lane_change_planner class BlockedByObstacleState : public StateBase { private: - geometry_msgs::PoseStamped current_pose_; - geometry_msgs::TwistStamped::ConstPtr current_twist_; - autoware_perception_msgs::DynamicObjectArray::ConstPtr dynamic_objects_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_twist_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects_; bool lane_change_approved_; bool force_lane_change_; bool found_safe_path_; @@ -49,9 +49,9 @@ class BlockedByObstacleState : public StateBase bool laneChangeForcedByOperator() const; // utility function - std::vector getBlockingObstacles() const; - autoware_planning_msgs::PathWithLaneId setStopPointFromObstacle( - const autoware_planning_msgs::PathWithLaneId & path); + std::vector getBlockingObstacles() const; + autoware_planning_msgs::msg::PathWithLaneId setStopPointFromObstacle( + const autoware_planning_msgs::msg::PathWithLaneId & path); public: BlockedByObstacleState( @@ -63,7 +63,7 @@ class BlockedByObstacleState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/common_functions.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/common_functions.h index d3007d1cc4f5f..79d5dd9d83355 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/common_functions.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/common_functions.h @@ -15,9 +15,9 @@ */ #pragma once -#include -#include -#include +#include +#include +#include #include #include #include @@ -32,24 +32,24 @@ bool selectLaneChangePath( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & dynamic_objects, - const geometry_msgs::Pose & current_pose, const geometry_msgs::Twist & current_twist, - const bool isInGoalRouteSection, const geometry_msgs::Pose & goal_pose, - const LaneChangerParameters & ros_parameters, LaneChangePath * path); + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr & dynamic_objects, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Twist & current_twist, + const bool isInGoalRouteSection, const geometry_msgs::msg::Pose & goal_pose, + const LaneChangerParameters & ros_parameters, LaneChangePath * path, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); bool isLaneChangePathSafe( - const autoware_planning_msgs::PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, + const autoware_planning_msgs::msg::PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & dynamic_objects, - const geometry_msgs::Pose & current_pose, const geometry_msgs::Twist & current_twist, - const LaneChangerParameters & ros_parameters, const bool use_buffer = true, + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr & dynamic_objects, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Twist & current_twist, + const LaneChangerParameters & ros_parameters, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock, const bool use_buffer = true, const double acceleration = 0.0); bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const geometry_msgs::Pose & current_pose, - const bool isInGoalRouteSection, const geometry_msgs::Pose & goal_pose, + const lanelet::ConstLanelets & target_lanes, const geometry_msgs::msg::Pose & current_pose, + const bool isInGoalRouteSection, const geometry_msgs::msg::Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs); -bool isObjectFront(const geometry_msgs::Pose & ego_pose, const geometry_msgs::Pose & obj_pose); +bool isObjectFront(const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & obj_pose); } // namespace common_functions } // namespace state_machine } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/executing_lane_change.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/executing_lane_change.h index f50727d71d6af..bd5d008330c3f 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/executing_lane_change.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/executing_lane_change.h @@ -17,12 +17,12 @@ #ifndef LANE_CHANGE_PLANNER_STATE_EXECUTING_LANE_CHANGE_H #define LANE_CHANGE_PLANNER_STATE_EXECUTING_LANE_CHANGE_H -#include +#include #include -#include -#include -#include +#include +#include +#include #include @@ -33,9 +33,9 @@ namespace lane_change_planner class ExecutingLaneChangeState : public StateBase { private: - geometry_msgs::PoseStamped current_pose_; - geometry_msgs::TwistStamped::ConstPtr current_twist_; - autoware_perception_msgs::DynamicObjectArray::ConstPtr dynamic_objects_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_twist_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects_; lanelet::ConstLanelets original_lanes_; lanelet::ConstLanelets target_lanes_; @@ -56,7 +56,7 @@ class ExecutingLaneChangeState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/following_lane.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/following_lane.h index 9062320b787e7..3cb6ff407dbce 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/following_lane.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/following_lane.h @@ -17,9 +17,9 @@ #ifndef LANE_CHANGE_PLANNER_STATE_FOLLOWING_LANE_H #define LANE_CHANGE_PLANNER_STATE_FOLLOWING_LANE_H -#include -#include -#include +#include +#include +#include #include #include @@ -30,9 +30,9 @@ namespace lane_change_planner class FollowingLaneState : public StateBase { private: - geometry_msgs::PoseStamped current_pose_; - geometry_msgs::TwistStamped::ConstPtr current_twist_; - autoware_perception_msgs::DynamicObjectArray::ConstPtr dynamic_objects_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_twist_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects_; bool lane_change_approved_; bool force_lane_change_; lanelet::ConstLanelets current_lanes_; @@ -58,7 +58,7 @@ class FollowingLaneState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/forcing_lane_change.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/forcing_lane_change.h index 696fabb33e15f..d926f290f0b5c 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/forcing_lane_change.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/forcing_lane_change.h @@ -19,9 +19,9 @@ #include -#include -#include -#include +#include +#include +#include #include @@ -32,9 +32,9 @@ class ForcingLaneChangeState : public StateBase private: // State transition conditions bool hasFinishedLaneChange() const; - geometry_msgs::PoseStamped current_pose_; - geometry_msgs::TwistStamped::ConstPtr current_twist_; - autoware_perception_msgs::DynamicObjectArray::ConstPtr dynamic_objects_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_twist_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects_; lanelet::ConstLanelets original_lanes_; lanelet::ConstLanelets target_lanes_; @@ -48,7 +48,7 @@ class ForcingLaneChangeState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/state_base_class.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/state_base_class.h index 691f338b2557c..1ed94d9ba10e1 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/state_base_class.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/state_base_class.h @@ -17,8 +17,8 @@ #ifndef LANE_CHANGE_PLANNER_STATE_STATE_BASE_CLASS_H #define LANE_CHANGE_PLANNER_STATE_STATE_BASE_CLASS_H -#include -#include +#include +#include #include #include #include @@ -40,7 +40,7 @@ std::ostream & operator<<(std::ostream & ostream, const State & state); struct Status { - autoware_planning_msgs::PathWithLaneId lane_follow_path; + autoware_planning_msgs::msg::PathWithLaneId lane_follow_path; LaneChangePath lane_change_path; std::vector lane_follow_lane_ids; std::vector lane_change_lane_ids; @@ -51,9 +51,9 @@ struct Status struct DebugData { std::vector lane_change_candidate_paths; - autoware_planning_msgs::PathWithLaneId selected_path; - autoware_planning_msgs::PathPointWithLaneId stop_point; - geometry_msgs::Point stop_factor_point; + autoware_planning_msgs::msg::PathWithLaneId selected_path; + autoware_planning_msgs::msg::PathPointWithLaneId stop_point; + geometry_msgs::msg::Point stop_factor_point; }; class StateBase @@ -73,7 +73,7 @@ class StateBase virtual void update() = 0; virtual State getNextState() const = 0; virtual State getCurrentState() const = 0; - virtual autoware_planning_msgs::PathWithLaneId getPath() const = 0; + virtual autoware_planning_msgs::msg::PathWithLaneId getPath() const = 0; Status getStatus() const; DebugData getDebugData() const; diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/stopping_lane_change.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/stopping_lane_change.h index d395d72ad86f6..70a59a5aee592 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/stopping_lane_change.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state/stopping_lane_change.h @@ -24,9 +24,9 @@ namespace lane_change_planner class StoppingLaneChangeState : public StateBase { private: - geometry_msgs::PoseStamped current_pose_; - geometry_msgs::TwistStamped::ConstPtr current_twist_; - autoware_perception_msgs::DynamicObjectArray::ConstPtr dynamic_objects_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_twist_; + autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr dynamic_objects_; lanelet::ConstLanelets original_lanes_; lanelet::ConstLanelets target_lanes_; @@ -35,8 +35,8 @@ class StoppingLaneChangeState : public StateBase bool isVehicleInOriginalLanes() const; // utility function - autoware_planning_msgs::PathWithLaneId setStopPoint( - const autoware_planning_msgs::PathWithLaneId & path); + autoware_planning_msgs::msg::PathWithLaneId setStopPoint( + const autoware_planning_msgs::msg::PathWithLaneId & path); public: StoppingLaneChangeState( @@ -48,7 +48,7 @@ class StoppingLaneChangeState : public StateBase void update() override; State getNextState() const override; State getCurrentState() const override; - autoware_planning_msgs::PathWithLaneId getPath() const override; + autoware_planning_msgs::msg::PathWithLaneId getPath() const override; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state_machine.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state_machine.h index c53b8a9f9fa0a..094bf66a818b3 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state_machine.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/state_machine.h @@ -17,12 +17,12 @@ #ifndef LANE_CHANGE_PLANNER_STATE_MACHINE_H #define LANE_CHANGE_PLANNER_STATE_MACHINE_H -#include -#include #include #include -#include +#include +#include #include +#include namespace lane_change_planner { @@ -40,9 +40,9 @@ class StateMachine const std::shared_ptr & data_manager_ptr, const std::shared_ptr & route_handler_ptr); void init(); - void init(const autoware_planning_msgs::Route & route); + void initCallback(const autoware_planning_msgs::msg::Route::ConstSharedPtr route); void updateState(); - autoware_planning_msgs::PathWithLaneId getPath() const; + autoware_planning_msgs::msg::PathWithLaneId getPath() const; Status getStatus() const; DebugData getDebugData() const; State getState() const; @@ -51,7 +51,6 @@ class StateMachine std::unique_ptr state_obj_ptr_; std::shared_ptr data_manager_ptr_; std::shared_ptr route_handler_ptr_; - ros::Publisher path_marker_publisher_; }; } // namespace lane_change_planner diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/utilities.h b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/utilities.h index 450d1a696a844..b527a48aadcdf 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/utilities.h +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/include/lane_change_planner/utilities.h @@ -17,15 +17,15 @@ #ifndef LANE_CHANGE_PLANNER_UTILITIES_H #define LANE_CHANGE_PLANNER_UTILITIES_H -#include -#include -#include -#include #include +#include +#include +#include +#include -#include -#include -#include +#include +#include +#include #include #include @@ -57,98 +57,102 @@ struct FrenetCoordinate3d }; double normalizeRadian(const double radian); -double l2Norm(const geometry_msgs::Vector3 vector); - -Eigen::Vector3d convertToEigenPt(const geometry_msgs::Point geom_pt); -std::vector convertToGeometryPointArray( - const autoware_planning_msgs::PathWithLaneId & path); -geometry_msgs::PoseArray convertToGeometryPoseArray( - const autoware_planning_msgs::PathWithLaneId & path); - -autoware_perception_msgs::PredictedPath convertToPredictedPath( - const autoware_planning_msgs::PathWithLaneId & path, const geometry_msgs::Twist & vehicle_twist, - const geometry_msgs::Pose & vehicle_pose, const double duration, const double resolution, - const double acceleration); -autoware_perception_msgs::PredictedPath resamplePredictedPath( - const autoware_perception_msgs::PredictedPath & input_path, const double resolution, - const double duration); +double l2Norm(const geometry_msgs::msg::Vector3 vector); + +Eigen::Vector3d convertToEigenPt(const geometry_msgs::msg::Point geom_pt); +std::vector convertToGeometryPointArray( + const autoware_planning_msgs::msg::PathWithLaneId & path); +geometry_msgs::msg::PoseArray convertToGeometryPoseArray( + const autoware_planning_msgs::msg::PathWithLaneId & path); + +autoware_perception_msgs::msg::PredictedPath convertToPredictedPath( + const autoware_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Twist & vehicle_twist, const geometry_msgs::msg::Pose & vehicle_pose, + const double duration, const double resolution, const double acceleration, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & input_path, const double resolution, + const double duration, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock); bool convertToFrenetCoordinate3d( - const std::vector & linestring, - const geometry_msgs::Point search_point_geom, FrenetCoordinate3d * frenet_coordinate); + const std::vector & linestring, + const geometry_msgs::msg::Point search_point_geom, FrenetCoordinate3d * frenet_coordinate); -geometry_msgs::Pose lerpByPose( - const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2, const double t); +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); -geometry_msgs::Point lerpByLength( - const std::vector & array, const double length); +geometry_msgs::msg::Point lerpByLength( + const std::vector & array, const double length); bool lerpByTimeStamp( - const autoware_perception_msgs::PredictedPath & path, const ros::Time & t, - geometry_msgs::Pose * lerped_pt); + const autoware_perception_msgs::msg::PredictedPath & path, const rclcpp::Time & t, + geometry_msgs::msg::Pose * lerped_pt, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr & clock); -double getDistance3d(const geometry_msgs::Point & p1, const geometry_msgs::Point & p2); +double getDistance3d(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); double getDistanceBetweenPredictedPaths( - const autoware_perception_msgs::PredictedPath & path1, - const autoware_perception_msgs::PredictedPath & path2, const double start_time, - const double end_time, const double resolution); + const autoware_perception_msgs::msg::PredictedPath & path1, + const autoware_perception_msgs::msg::PredictedPath & path2, const double start_time, + const double end_time, const double resolution, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr & clock); std::vector filterObjectsByLanelets( - const autoware_perception_msgs::DynamicObjectArray & objects, + const autoware_perception_msgs::msg::DynamicObjectArray & objects, const lanelet::ConstLanelets & lanelets, const double start_arc_length, - const double end_arc_length); + const double end_arc_length, const rclcpp::Logger & logger); std::vector filterObjectsByLanelets( - const autoware_perception_msgs::DynamicObjectArray & objects, - const lanelet::ConstLanelets & target_lanelets); + const autoware_perception_msgs::msg::DynamicObjectArray & objects, + const lanelet::ConstLanelets & target_lanelets, const rclcpp::Logger & logger); bool calcObjectPolygon( - const autoware_perception_msgs::DynamicObject & object, Polygon * object_polygon); + const autoware_perception_msgs::msg::DynamicObject & object, Polygon * object_polygon, + const rclcpp::Logger & logger); std::vector filterObjectsByPath( - const autoware_perception_msgs::DynamicObjectArray & objects, + const autoware_perception_msgs::msg::DynamicObjectArray & objects, const std::vector & object_indices, - const autoware_planning_msgs::PathWithLaneId & ego_path, const double vehicle_width); + const autoware_planning_msgs::msg::PathWithLaneId & ego_path, const double vehicle_width, + const rclcpp::Logger & logger); -const geometry_msgs::Pose refineGoal( - const geometry_msgs::Pose & goal, const lanelet::ConstLanelet & goal_lanelet); +const geometry_msgs::msg::Pose refineGoal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet); -autoware_planning_msgs::PathWithLaneId refinePath( +autoware_planning_msgs::msg::PathWithLaneId refinePath( const double search_radius_range, const double search_rad_range, - const autoware_planning_msgs::PathWithLaneId & input, const geometry_msgs::Pose & goal, - const int64_t goal_lane_id); -autoware_planning_msgs::PathWithLaneId removeOverlappingPoints( - const autoware_planning_msgs::PathWithLaneId & input_path); + const autoware_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Pose & goal, + const int64_t goal_lane_id, const rclcpp::Logger & logger); +autoware_planning_msgs::msg::PathWithLaneId removeOverlappingPoints( + const autoware_planning_msgs::msg::PathWithLaneId & input_path); bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); -nav_msgs::OccupancyGrid generateDrivableArea( - const lanelet::ConstLanelets & lanes, const geometry_msgs::PoseStamped & current_pose, +nav_msgs::msg::OccupancyGrid generateDrivableArea( + const lanelet::ConstLanelets & lanes, const geometry_msgs::msg::PoseStamped & current_pose, const double width, const double height, const double resolution, const double vehicle_length, const RouteHandler & route_handler); double getDistanceToEndOfLane( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets); + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets); double getDistanceToNextIntersection( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets); + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets); double getDistanceToCrosswalk( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets, + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphContainer & overall_graphs); double getSignedDistance( - const geometry_msgs::Pose & current_pose, const geometry_msgs::Pose & goal_pose, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose, const lanelet::ConstLanelets & lanelets); std::vector getIds(const lanelet::ConstLanelets & lanelets); -autoware_planning_msgs::Path convertToPathFromPathWithLaneId( - const autoware_planning_msgs::PathWithLaneId & path_with_lane_id); +autoware_planning_msgs::msg::Path convertToPathFromPathWithLaneId( + const autoware_planning_msgs::msg::PathWithLaneId & path_with_lane_id); lanelet::Polygon3d getVehiclePolygon( - const geometry_msgs::Pose & vehicle_pose, const double vehicle_width, + const geometry_msgs::msg::Pose & vehicle_pose, const double vehicle_width, const double base_link2front); -autoware_planning_msgs::PathPointWithLaneId insertStopPoint( - double length, autoware_planning_msgs::PathWithLaneId * path); +autoware_planning_msgs::msg::PathPointWithLaneId insertStopPoint( + double length, autoware_planning_msgs::msg::PathWithLaneId * path); class SplineInterpolate { @@ -159,8 +163,10 @@ class SplineInterpolate std::vector d_; std::vector h_; + rclcpp::Logger logger_; + public: - SplineInterpolate(); + SplineInterpolate(const rclcpp::Logger & logger); bool interpolate( const std::vector & base_index, const std::vector & base_value, const std::vector & return_index, std::vector & return_value); diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/package.xml b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/package.xml index 33128c934125e..35d5010bc40a2 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/package.xml +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/package.xml @@ -8,15 +8,20 @@ TODO - catkin + ament_cmake_auto + autoware_perception_msgs autoware_planning_msgs geometry_msgs lanelet2_extension - roscpp + rclcpp sensor_msgs tf2 tf2_geometry_msgs tf2_ros libopencv-dev + + + ament_cmake + diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/data_manager.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/data_manager.cpp index 1d9fe0a39bf14..4c6367801c2d5 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/data_manager.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/data_manager.cpp @@ -19,34 +19,39 @@ namespace lane_change_planner { -DataManager::DataManager() -: is_parameter_set_(false), lane_change_approval_(false), force_lane_change_(false) +DataManager::DataManager(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) +: lane_change_approval_(false), + force_lane_change_(false), + is_parameter_set_(false), + logger_(logger), + clock_(clock) { - self_pose_listener_ptr_ = std::make_shared(); + self_pose_listener_ptr_ = std::make_shared(logger, clock); } void DataManager::perceptionCallback( - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & input_perception_msg_ptr) + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_perception_msg_ptr) { perception_ptr_ = input_perception_msg_ptr; } void DataManager::velocityCallback( - const geometry_msgs::TwistStamped::ConstPtr & input_twist_msg_ptr) + const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_twist_msg_ptr) { vehicle_velocity_ptr_ = input_twist_msg_ptr; } -void DataManager::laneChangeApprovalCallback(const std_msgs::Bool & input_approval_msg) +void DataManager::laneChangeApprovalCallback(const std_msgs::msg::Bool::ConstSharedPtr input_approval_msg) { - lane_change_approval_.data = input_approval_msg.data; - lane_change_approval_.stamp = ros::Time::now(); + lane_change_approval_.data = input_approval_msg->data; + lane_change_approval_.stamp = clock_->now(); } -void DataManager::forceLaneChangeSignalCallback(const std_msgs::Bool & input_force_lane_change_msg) +void DataManager::forceLaneChangeSignalCallback( + const std_msgs::msg::Bool::ConstSharedPtr input_force_lane_change_msg) { - force_lane_change_.data = input_force_lane_change_msg.data; - force_lane_change_.stamp = ros::Time::now(); + force_lane_change_.data = input_force_lane_change_msg->data; + force_lane_change_.stamp = clock_->now(); } void DataManager::setLaneChangerParameters(const LaneChangerParameters & parameters) @@ -55,17 +60,17 @@ void DataManager::setLaneChangerParameters(const LaneChangerParameters & paramet parameters_ = parameters; } -autoware_perception_msgs::DynamicObjectArray::ConstPtr DataManager::getDynamicObjects() +autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr DataManager::getDynamicObjects() { return perception_ptr_; } -geometry_msgs::TwistStamped::ConstPtr DataManager::getCurrentSelfVelocity() +geometry_msgs::msg::TwistStamped::ConstSharedPtr DataManager::getCurrentSelfVelocity() { return vehicle_velocity_ptr_; } -geometry_msgs::PoseStamped DataManager::getCurrentSelfPose() +geometry_msgs::msg::PoseStamped DataManager::getCurrentSelfPose() { self_pose_listener_ptr_->getSelfPose(self_pose_); return self_pose_; @@ -76,7 +81,7 @@ LaneChangerParameters DataManager::getLaneChangerParameters() { return parameter bool DataManager::getLaneChangeApproval() { constexpr double timeout = 0.5; - if (ros::Time::now() - lane_change_approval_.stamp > ros::Duration(timeout)) { + if (clock_->now() - lane_change_approval_.stamp > rclcpp::Duration::from_seconds(timeout)) { return false; } @@ -86,13 +91,23 @@ bool DataManager::getLaneChangeApproval() bool DataManager::getForceLaneChangeSignal() { constexpr double timeout = 0.5; - if (ros::Time::now() - force_lane_change_.stamp > ros::Duration(timeout)) { + if (clock_->now() - force_lane_change_.stamp > rclcpp::Duration::from_seconds(timeout)) { return false; } else { return force_lane_change_.data; } } +rclcpp::Logger & DataManager::getLogger() +{ + return logger_; +} + +rclcpp::Clock::SharedPtr DataManager::getClock() +{ + return clock_; +} + bool DataManager::isDataReady() { if (!perception_ptr_) { @@ -107,20 +122,21 @@ bool DataManager::isDataReady() return true; } -SelfPoseLinstener::SelfPoseLinstener() : tf_listener_(tf_buffer_){}; +SelfPoseLinstener::SelfPoseLinstener(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) +: tf_listener_(tf_buffer_), logger_(logger), clock_(clock){} bool SelfPoseLinstener::isSelfPoseReady() { - return tf_buffer_.canTransform("map", "base_link", ros::Time(0), ros::Duration(0.1)); + return tf_buffer_.canTransform("map", "base_link", tf2::TimePointZero); } -bool SelfPoseLinstener::getSelfPose(geometry_msgs::PoseStamped & self_pose) +bool SelfPoseLinstener::getSelfPose(geometry_msgs::msg::PoseStamped & self_pose) { try { - geometry_msgs::TransformStamped transform; + geometry_msgs::msg::TransformStamped transform; std::string map_frame = "map"; transform = - tf_buffer_.lookupTransform(map_frame, "base_link", ros::Time(0), ros::Duration(0.1)); + tf_buffer_.lookupTransform(map_frame, "base_link", tf2::TimePointZero); self_pose.pose.position.x = transform.transform.translation.x; self_pose.pose.position.y = transform.transform.translation.y; self_pose.pose.position.z = transform.transform.translation.z; @@ -128,11 +144,11 @@ bool SelfPoseLinstener::getSelfPose(geometry_msgs::PoseStamped & self_pose) self_pose.pose.orientation.y = transform.transform.rotation.y; self_pose.pose.orientation.z = transform.transform.rotation.z; self_pose.pose.orientation.w = transform.transform.rotation.w; - self_pose.header.stamp = ros::Time::now(); + self_pose.header.stamp = clock_->now(); self_pose.header.frame_id = map_frame; return true; } catch (tf2::TransformException & ex) { - ROS_ERROR_STREAM_THROTTLE(1, "failed to find self pose :" << ex.what()); + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1.0, "failed to find self pose :" << ex.what()); return false; } } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_change_planner_node.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_change_planner_node.cpp index b242551bf6cb4..ffbc85c455a3f 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_change_planner_node.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_change_planner_node.cpp @@ -15,15 +15,15 @@ */ #include -#include +#include int main(int argc, char ** argv) { - ros::init(argc, argv, "lane_change_planner_node"); + rclcpp::init(argc, argv); - lane_change_planner::LaneChanger lane_changer; + rclcpp::spin(std::make_shared()); - ros::spin(); + rclcpp::shutdown(); return 0; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_changer.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_changer.cpp index 21d33042fb585..4496ed5ded665 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_changer.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/lane_changer.cpp @@ -16,80 +16,78 @@ #include #include -#include -std_msgs::ColorRGBA toRainbow(double ratio); -visualization_msgs::Marker convertToMarker( - const autoware_perception_msgs::PredictedPath & path, const int id, const std::string & ns, - const double radius); +std_msgs::msg::ColorRGBA toRainbow(double ratio); +visualization_msgs::msg::Marker convertToMarker( + const autoware_perception_msgs::msg::PredictedPath & path, const int id, const std::string & ns, + const double radius, const rclcpp::Time & timestamp); -visualization_msgs::Marker convertToMarker( - const autoware_planning_msgs::PathWithLaneId & path, const int id, const std::string & ns, - const std_msgs::ColorRGBA & color); +visualization_msgs::msg::Marker convertToMarker( + const autoware_planning_msgs::msg::PathWithLaneId & path, const int id, const std::string & ns, + const std_msgs::msg::ColorRGBA & color, const rclcpp::Time & timestamp); -visualization_msgs::MarkerArray createVirtualWall( - const geometry_msgs::Pose & pose, const int id, const std::string & factor_text); +visualization_msgs::msg::MarkerArray createVirtualWall( + const geometry_msgs::msg::Pose & pose, const int id, const std::string & factor_text, const rclcpp::Time & timestamp); namespace lane_change_planner { -LaneChanger::LaneChanger() : pnh_("~") { init(); } +LaneChanger::LaneChanger() : Node("lane_change_planner_node"){ init(); } void LaneChanger::init() { // data_manager - data_manager_ptr_ = std::make_shared(); - route_handler_ptr_ = std::make_shared(); + data_manager_ptr_ = std::make_shared(get_logger(), get_clock()); + route_handler_ptr_ = std::make_shared(get_logger(), get_clock()); velocity_subscriber_ = - pnh_.subscribe("input/velocity", 1, &DataManager::velocityCallback, &(*data_manager_ptr_)); + create_subscription("input/velocity", rclcpp::QoS{1}, std::bind(&DataManager::velocityCallback, &(*data_manager_ptr_), std::placeholders::_1)); perception_subscriber_ = - pnh_.subscribe("input/perception", 1, &DataManager::perceptionCallback, &(*data_manager_ptr_)); - lane_change_approval_subscriber_ = pnh_.subscribe( - "input/lane_change_approval", 1, &DataManager::laneChangeApprovalCallback, - &(*data_manager_ptr_)); - force_lane_change_subscriber_ = pnh_.subscribe( - "input/force_lane_change", 1, &DataManager::forceLaneChangeSignalCallback, - &(*data_manager_ptr_)); + create_subscription("input/perception", rclcpp::QoS{1}, std::bind(&DataManager::perceptionCallback, &(*data_manager_ptr_), std::placeholders::_1)); + lane_change_approval_subscriber_ = create_subscription( + "input/lane_change_approval", rclcpp::QoS{1}, std::bind(&DataManager::laneChangeApprovalCallback, + &(*data_manager_ptr_), std::placeholders::_1)); + force_lane_change_subscriber_ = create_subscription( + "input/force_lane_change", rclcpp::QoS{1}, std::bind(&DataManager::forceLaneChangeSignalCallback, + &(*data_manager_ptr_), std::placeholders::_1)); // ROS parameters LaneChangerParameters parameters; - pnh_.param("min_stop_distance", parameters.min_stop_distance, 5.0); - pnh_.param("stop_time", parameters.stop_time, 2.0); - pnh_.param("hysteresis_buffer_distance", parameters.hysteresis_buffer_distance, 2.0); - pnh_.param("backward_path_length", parameters.backward_path_length, 5.0); - pnh_.param("forward_path_length", parameters.forward_path_length, 100.0); - pnh_.param("lane_change_prepare_duration", parameters.lane_change_prepare_duration, 2.0); - pnh_.param("lane_changing_duration", parameters.lane_changing_duration, 4.0); - pnh_.param("minimum_lane_change_length", parameters.minimum_lane_change_length, 8.0); - pnh_.param("prediction_duration", parameters.prediction_duration, 8.0); - pnh_.param("prediction_time_resolution", parameters.prediction_time_resolution, 0.5); - pnh_.param("drivable_area_resolution", parameters.drivable_area_resolution, 0.1); - pnh_.param("drivable_area_width", parameters.drivable_area_width, 100.0); - pnh_.param("drivable_area_height", parameters.drivable_area_height, 50.0); - pnh_.param("static_obstacle_velocity_thresh", parameters.static_obstacle_velocity_thresh, 0.1); - pnh_.param("maximum_deceleration", parameters.maximum_deceleration, 1.0); - pnh_.param("lane_change_sampling_num", parameters.lane_change_sampling_num, 10); - pnh_.param("enable_abort_lane_change", parameters.enable_abort_lane_change, true); - pnh_.param("/vehicle_info/vehicle_width", parameters.vehicle_width, 2.8); - pnh_.param("/vehicle_info/vehicle_length", parameters.vehicle_length, 5.0); - pnh_.param("/vehicle_info/max_longitudinal_offset", parameters.base_link2front, 3.74); - pnh_.param( - "abort_lane_change_velocity_thresh", parameters.abort_lane_change_velocity_thresh, 0.5); - pnh_.param( - "abort_lane_change_angle_thresh", parameters.abort_lane_change_angle_thresh, - 0.174533); // 10 deg - pnh_.param( - "abort_lane_change_distance_thresh", parameters.abort_lane_change_distance_thresh, 0.3); + parameters.min_stop_distance = declare_parameter("min_stop_distance", 5.0); + parameters.stop_time = declare_parameter("stop_time", 2.0); + parameters.hysteresis_buffer_distance = declare_parameter("hysteresis_buffer_distance", 2.0); + parameters.backward_path_length = declare_parameter("backward_path_length", 5.0); + parameters.forward_path_length = declare_parameter("forward_path_length", 100.0); + parameters.lane_change_prepare_duration = declare_parameter("lane_change_prepare_duration", 2.0); + parameters.lane_changing_duration = declare_parameter("lane_changing_duration", 4.0); + parameters.minimum_lane_change_length = declare_parameter("minimum_lane_change_length", 8.0); + parameters.prediction_duration = declare_parameter("prediction_duration", 8.0); + parameters.prediction_time_resolution = declare_parameter("prediction_time_resolution", 0.5); + parameters.drivable_area_resolution = declare_parameter("drivable_area_resolution", 0.1); + parameters.drivable_area_width = declare_parameter("drivable_area_width", 100.0); + parameters.drivable_area_height = declare_parameter("drivable_area_height", 50.0); + parameters.static_obstacle_velocity_thresh = declare_parameter("static_obstacle_velocity_thresh", 0.1); + parameters.maximum_deceleration = declare_parameter("maximum_deceleration", 1.0); + parameters.lane_change_sampling_num = declare_parameter("lane_change_sampling_num", 10); + parameters.enable_abort_lane_change = declare_parameter("enable_abort_lane_change", true); + parameters.vehicle_width = declare_parameter("/vehicle_info/vehicle_width", 2.8); + parameters.vehicle_length = declare_parameter("/vehicle_info/vehicle_length", 5.0); + parameters.base_link2front = declare_parameter("/vehicle_info/max_longitudinal_offset", 3.74); + parameters.abort_lane_change_velocity_thresh = declare_parameter( + "abort_lane_change_velocity_thresh", 0.5); + parameters.abort_lane_change_angle_thresh = declare_parameter( + "abort_lane_change_angle_thresh", 0.174533); // 10 deg + parameters.abort_lane_change_distance_thresh = declare_parameter( + "abort_lane_change_distance_thresh", 0.3); // validation of parameters if (parameters.lane_change_sampling_num < 1) { - ROS_FATAL_STREAM( + RCLCPP_FATAL_STREAM(get_logger(), "lane_change_sampling_num must be positive integer. Given parameter: " << parameters.lane_change_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } if (parameters.maximum_deceleration < 0.0) { - ROS_FATAL_STREAM( + RCLCPP_FATAL_STREAM(get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " << parameters.maximum_deceleration << std::endl << "Terminating the progam..."); @@ -99,59 +97,58 @@ void LaneChanger::init() // route_handler vector_map_subscriber_ = - pnh_.subscribe("input/vector_map", 1, &RouteHandler::mapCallback, &(*route_handler_ptr_)); + create_subscription("input/vector_map", rclcpp::QoS{1}, std::bind(&RouteHandler::mapCallback, &(*route_handler_ptr_), std::placeholders::_1)); route_subscriber_ = - pnh_.subscribe("input/route", 1, &RouteHandler::routeCallback, &(*route_handler_ptr_)); + create_subscription("input/route", rclcpp::QoS{1}, std::bind(&RouteHandler::routeCallback, &(*route_handler_ptr_), std::placeholders::_1)); // publisher path_publisher_ = - pnh_.advertise("output/lane_change_path", 1); + create_publisher("output/lane_change_path", rclcpp::QoS{1}); candidate_path_publisher_ = - pnh_.advertise("debug/lane_change_candidate_path", 1); + create_publisher("debug/lane_change_candidate_path", rclcpp::QoS{1}); path_marker_publisher_ = - pnh_.advertise("debug/predicted_path_markers", 1); + create_publisher("debug/predicted_path_markers", rclcpp::QoS{1}); stop_reason_publisher_ = - pnh_.advertise("output/stop_reasons", 1); - drivable_area_publisher_ = pnh_.advertise("debug/drivable_area", 1); - lane_change_ready_publisher_ = pnh_.advertise("output/lane_change_ready", 1); + create_publisher("output/stop_reasons", rclcpp::QoS{1}); + drivable_area_publisher_ = create_publisher("debug/drivable_area", rclcpp::QoS{1}); + lane_change_ready_publisher_ = create_publisher("output/lane_change_ready", rclcpp::QoS{1}); lane_change_available_publisher_ = - pnh_.advertise("output/lane_change_available", 1); - - waitForData(); + create_publisher("output/lane_change_available", rclcpp::QoS{1}); // set state_machine state_machine_ptr_ = std::make_shared(data_manager_ptr_, route_handler_ptr_); state_machine_ptr_->init(); route_init_subscriber_ = - pnh_.subscribe("input/route", 1, &StateMachine::init, &(*state_machine_ptr_)); + create_subscription("input/route", rclcpp::QoS{1}, std::bind(&StateMachine::initCallback, &(*state_machine_ptr_), std::placeholders::_1)); // Start timer. This must be done after all data (e.g. vehicle pose, velocity) are ready. - timer_ = pnh_.createTimer(ros::Duration(0.1), &LaneChanger::run, this); + auto timer_callback = std::bind(&LaneChanger::run, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(0.1)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); } -void LaneChanger::waitForData() +void LaneChanger::run() { // wait until mandatory data is ready - while (!route_handler_ptr_->isHandlerReady() && ros::ok()) { - ROS_WARN_THROTTLE(5, "waiting for route to be ready"); - ros::spinOnce(); - ros::Duration(0.1).sleep(); + if (!route_handler_ptr_->isHandlerReady()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5.0, "waiting for route to be ready"); + return; } - while (!data_manager_ptr_->isDataReady() && ros::ok()) { - ROS_WARN_THROTTLE(5, "waiting for vehicle pose, vehicle_velocity, and obstacles"); - ros::spinOnce(); - ros::Duration(0.1).sleep(); + if (!data_manager_ptr_->isDataReady()) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5.0, "waiting for vehicle pose, vehicle_velocity, and obstacles"); + return; } -} -void LaneChanger::run(const ros::TimerEvent & event) -{ state_machine_ptr_->updateState(); const auto path = state_machine_ptr_->getPath(); const auto goal = route_handler_ptr_->getGoalPose(); const auto goal_lane_id = route_handler_ptr_->getGoalLaneId(); - geometry_msgs::Pose refined_goal; + geometry_msgs::msg::Pose refined_goal; { lanelet::ConstLanelet goal_lanelet; if (route_handler_ptr_->getGoalLanelet(&goal_lanelet)) { @@ -161,12 +158,12 @@ void LaneChanger::run(const ros::TimerEvent & event) } } - auto refined_path = util::refinePath(7.5, M_PI * 0.5, path, refined_goal, goal_lane_id); + auto refined_path = util::refinePath(7.5, M_PI * 0.5, path, refined_goal, goal_lane_id, this->get_logger()); refined_path.header.frame_id = "map"; - refined_path.header.stamp = ros::Time::now(); + refined_path.header.stamp = this->now(); if (!path.points.empty()) { - path_publisher_.publish(refined_path); + path_publisher_->publish(refined_path); } publishDebugMarkers(); @@ -174,24 +171,24 @@ void LaneChanger::run(const ros::TimerEvent & event) // publish lane change status const auto lane_change_status = state_machine_ptr_->getStatus(); - std_msgs::Bool lane_change_ready_msg; + std_msgs::msg::Bool lane_change_ready_msg; lane_change_ready_msg.data = lane_change_status.lane_change_ready; - lane_change_ready_publisher_.publish(lane_change_ready_msg); + lane_change_ready_publisher_->publish(lane_change_ready_msg); - std_msgs::Bool lane_change_available_msg; + std_msgs::msg::Bool lane_change_available_msg; lane_change_available_msg.data = lane_change_status.lane_change_available; - lane_change_available_publisher_.publish(lane_change_available_msg); + lane_change_available_publisher_->publish(lane_change_available_msg); auto candidate_path = util::convertToPathFromPathWithLaneId(lane_change_status.lane_change_path.path); candidate_path.header.frame_id = "map"; - candidate_path.header.stamp = ros::Time::now(); - candidate_path_publisher_.publish(candidate_path); + candidate_path.header.stamp = this->now(); + candidate_path_publisher_->publish(candidate_path); } -void LaneChanger::publishDrivableArea(const autoware_planning_msgs::PathWithLaneId & path) +void LaneChanger::publishDrivableArea(const autoware_planning_msgs::msg::PathWithLaneId & path) { - drivable_area_publisher_.publish(path.drivable_area); + drivable_area_publisher_->publish(path.drivable_area); } void LaneChanger::publishDebugMarkers() @@ -206,33 +203,33 @@ void LaneChanger::publishDebugMarkers() const double time_resolution = ros_parameters.prediction_time_resolution; const double prediction_duration = ros_parameters.prediction_duration; - visualization_msgs::MarkerArray debug_markers; - autoware_planning_msgs::StopReasonArray stop_reason_array; + visualization_msgs::msg::MarkerArray debug_markers; + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; // get ego vehicle path marker const auto & status = state_machine_ptr_->getStatus(); if (!status.lane_change_path.path.points.empty()) { const auto & vehicle_predicted_path = util::convertToPredictedPath( status.lane_change_path.path, current_twist->twist, current_pose.pose, prediction_duration, - time_resolution, 0); + time_resolution, 0, this->get_logger(), this->get_clock()); const auto & resampled_path = - util::resamplePredictedPath(vehicle_predicted_path, time_resolution, prediction_duration); + util::resamplePredictedPath(vehicle_predicted_path, time_resolution, prediction_duration, this->get_logger(), this->get_clock()); double radius = util::l2Norm(current_twist->twist.linear) * stop_time; radius = std::max(radius, min_radius); - const auto & marker = convertToMarker(resampled_path, 1, "ego_lane_change_path", radius); + const auto & marker = convertToMarker(resampled_path, 1, "ego_lane_change_path", radius, this->now()); debug_markers.markers.push_back(marker); } if (!status.lane_follow_path.points.empty()) { const auto & vehicle_predicted_path = util::convertToPredictedPath( status.lane_follow_path, current_twist->twist, current_pose.pose, prediction_duration, - time_resolution, 0.0); + time_resolution, 0.0, this->get_logger(), this->get_clock()); const auto & resampled_path = - util::resamplePredictedPath(vehicle_predicted_path, time_resolution, prediction_duration); + util::resamplePredictedPath(vehicle_predicted_path, time_resolution, prediction_duration, this->get_logger(), this->get_clock()); double radius = util::l2Norm(current_twist->twist.linear) * stop_time; radius = std::max(radius, min_radius); - const auto & marker = convertToMarker(resampled_path, 1, "ego_lane_follow_path", radius); + const auto & marker = convertToMarker(resampled_path, 1, "ego_lane_follow_path", radius, this->now()); debug_markers.markers.push_back(marker); } @@ -244,15 +241,15 @@ void LaneChanger::publishDebugMarkers() const auto & check_lanes = route_handler_ptr_->getCheckTargetLanesFromPath( status.lane_change_path.path, target_lanes, check_distance); - const auto object_indices = util::filterObjectsByLanelets(*dynamic_objects, check_lanes); + const auto object_indices = util::filterObjectsByLanelets(*dynamic_objects, check_lanes, get_logger()); for (const auto & i : object_indices) { const auto & obj = dynamic_objects->objects.at(i); for (const auto & obj_path : obj.state.predicted_paths) { const auto & resampled_path = - util::resamplePredictedPath(obj_path, time_resolution, prediction_duration); + util::resamplePredictedPath(obj_path, time_resolution, prediction_duration, this->get_logger(), this->get_clock()); double radius = util::l2Norm(obj.state.twist_covariance.twist.linear) * stop_time; radius = std::max(radius, min_radius); - const auto & marker = convertToMarker(resampled_path, i, "object_predicted_path", radius); + const auto & marker = convertToMarker(resampled_path, i, "object_predicted_path", radius, this->now()); debug_markers.markers.push_back(marker); } } @@ -262,20 +259,20 @@ void LaneChanger::publishDebugMarkers() // candidate paths { int i = 0; - std_msgs::ColorRGBA color; + std_msgs::msg::ColorRGBA color; color.r = 1; color.g = 1; color.b = 1; color.a = 0.6; for (const auto & path : debug_data.lane_change_candidate_paths) { - const auto marker = convertToMarker(path.path, i++, "candidate_lane_change_path", color); + const auto marker = convertToMarker(path.path, i++, "candidate_lane_change_path", color, this->now()); debug_markers.markers.push_back(marker); } color.r = 1; color.g = 0; color.b = 0; color.a = 0.9; - const auto marker = convertToMarker(debug_data.selected_path, 1, "selected_path", color); + const auto marker = convertToMarker(debug_data.selected_path, 1, "selected_path", color, this->now()); debug_markers.markers.push_back(marker); } @@ -283,7 +280,7 @@ void LaneChanger::publishDebugMarkers() { if (state_machine_ptr_->getState() == State::BLOCKED_BY_OBSTACLE) { // calculate virtual wall pose - geometry_msgs::Pose wall_pose; + geometry_msgs::msg::Pose wall_pose; tf2::Transform tf_map2base_link; tf2::fromMsg(debug_data.stop_point.point.pose, tf_map2base_link); tf2::Transform tf_base_link2front( @@ -291,7 +288,7 @@ void LaneChanger::publishDebugMarkers() tf2::Vector3(ros_parameters.base_link2front, 0.0, 0.0)); tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; tf2::toMsg(tf_map2front, wall_pose); - const auto virtual_wall_markers = createVirtualWall(wall_pose, 1, "blockedByObstacle"); + const auto virtual_wall_markers = createVirtualWall(wall_pose, 1, "blockedByObstacle", this->now()); debug_markers.markers.insert( debug_markers.markers.end(), virtual_wall_markers.markers.begin(), virtual_wall_markers.markers.end()); @@ -302,7 +299,7 @@ void LaneChanger::publishDebugMarkers() { if (state_machine_ptr_->getState() == State::STOPPING_LANE_CHANGE) { // calculate virtual wall pose - geometry_msgs::Pose wall_pose; + geometry_msgs::msg::Pose wall_pose; tf2::Transform tf_map2base_link; tf2::fromMsg(debug_data.stop_point.point.pose, tf_map2base_link); tf2::Transform tf_base_link2front( @@ -310,7 +307,7 @@ void LaneChanger::publishDebugMarkers() tf2::Vector3(ros_parameters.base_link2front, 0.0, 0.0)); tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; tf2::toMsg(tf_map2front, wall_pose); - const auto virtual_wall_markers = createVirtualWall(wall_pose, 1, "Stopping"); + const auto virtual_wall_markers = createVirtualWall(wall_pose, 1, "Stopping", this->now()); debug_markers.markers.insert( debug_markers.markers.end(), virtual_wall_markers.markers.begin(), virtual_wall_markers.markers.end()); @@ -320,30 +317,30 @@ void LaneChanger::publishDebugMarkers() //create stop reason array from debug_data and state stop_reason_array = makeStopReasonArray(debug_data, state_machine_ptr_->getState()); - path_marker_publisher_.publish(debug_markers); - stop_reason_publisher_.publish(stop_reason_array); + path_marker_publisher_->publish(debug_markers); + stop_reason_publisher_->publish(stop_reason_array); } -autoware_planning_msgs::StopReasonArray LaneChanger::makeStopReasonArray( +autoware_planning_msgs::msg::StopReasonArray LaneChanger::makeStopReasonArray( const DebugData & debug_data, const State & state) { //create header - std_msgs::Header header; + std_msgs::msg::Header header; header.frame_id = "map"; - header.stamp = ros::Time::now(); + header.stamp = this->now(); //create stop reason array - autoware_planning_msgs::StopReasonArray stop_reason_array; + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; stop_reason_array.header = header; //create stop reason stamped - autoware_planning_msgs::StopReason stop_reason_msg; - autoware_planning_msgs::StopFactor stop_factor; + autoware_planning_msgs::msg::StopReason stop_reason_msg; + autoware_planning_msgs::msg::StopFactor stop_factor; if (state == lane_change_planner::State::BLOCKED_BY_OBSTACLE) { - stop_reason_msg.reason = autoware_planning_msgs::StopReason::BLOCKED_BY_OBSTACLE; + stop_reason_msg.reason = autoware_planning_msgs::msg::StopReason::BLOCKED_BY_OBSTACLE; } else if (state == lane_change_planner::State::STOPPING_LANE_CHANGE) { - stop_reason_msg.reason = autoware_planning_msgs::StopReason::STOPPING_LANE_CHANGE; + stop_reason_msg.reason = autoware_planning_msgs::msg::StopReason::STOPPING_LANE_CHANGE; } else { //not stop. return empty reason_point stop_reason_array.stop_reasons = makeEmptyStopReasons(); @@ -361,14 +358,14 @@ autoware_planning_msgs::StopReasonArray LaneChanger::makeStopReasonArray( return stop_reason_array; } -std::vector LaneChanger::makeEmptyStopReasons() +std::vector LaneChanger::makeEmptyStopReasons() { - autoware_planning_msgs::StopReason stop_reason_blocked; - stop_reason_blocked.reason = autoware_planning_msgs::StopReason::BLOCKED_BY_OBSTACLE; - autoware_planning_msgs::StopReason stop_reason_stopping; - stop_reason_stopping.reason = autoware_planning_msgs::StopReason::STOPPING_LANE_CHANGE; + autoware_planning_msgs::msg::StopReason stop_reason_blocked; + stop_reason_blocked.reason = autoware_planning_msgs::msg::StopReason::BLOCKED_BY_OBSTACLE; + autoware_planning_msgs::msg::StopReason stop_reason_stopping; + stop_reason_stopping.reason = autoware_planning_msgs::msg::StopReason::STOPPING_LANE_CHANGE; - std::vector stop_reasons; + std::vector stop_reasons; stop_reasons.emplace_back(stop_reason_blocked); stop_reasons.emplace_back(stop_reason_stopping); return stop_reasons; @@ -376,7 +373,7 @@ std::vector LaneChanger::makeEmptyStopReason } // namespace lane_change_planner -std_msgs::ColorRGBA toRainbow(double ratio) +std_msgs::msg::ColorRGBA toRainbow(double ratio) { // we want to normalize ratio so that it fits in to 6 regions // where each region is 256 units long @@ -418,7 +415,7 @@ std_msgs::ColorRGBA toRainbow(double ratio) blu = 255 - x; break; // magenta } - std_msgs::ColorRGBA color; + std_msgs::msg::ColorRGBA color; color.r = static_cast(red) / 255; color.g = static_cast(grn) / 255; color.b = static_cast(blu) / 255; @@ -427,17 +424,17 @@ std_msgs::ColorRGBA toRainbow(double ratio) return color; } -visualization_msgs::Marker convertToMarker( - const autoware_perception_msgs::PredictedPath & path, const int id, const std::string & ns, - const double radius) +visualization_msgs::msg::Marker convertToMarker( + const autoware_perception_msgs::msg::PredictedPath & path, const int id, const std::string & ns, + const double radius, const rclcpp::Time & timestamp) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = timestamp; marker.ns = ns; marker.id = id; - marker.type = visualization_msgs::Marker::SPHERE_LIST; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; @@ -456,7 +453,7 @@ visualization_msgs::Marker convertToMarker( marker.color.b = 1.0; marker.color.a = 0.3; - marker.lifetime = ros::Duration(1); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); marker.frame_locked = true; for (std::size_t i = 0; i < path.path.size(); i++) { @@ -467,17 +464,17 @@ visualization_msgs::Marker convertToMarker( return marker; } -visualization_msgs::Marker convertToMarker( - const autoware_planning_msgs::PathWithLaneId & path, const int id, const std::string & ns, - const std_msgs::ColorRGBA & color) +visualization_msgs::msg::Marker convertToMarker( + const autoware_planning_msgs::msg::PathWithLaneId & path, const int id, const std::string & ns, + const std_msgs::msg::ColorRGBA & color, const rclcpp::Time & timestamp) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = timestamp; marker.ns = ns; marker.id = id; - marker.type = visualization_msgs::Marker::LINE_STRIP; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.position.x = 0.0; marker.pose.position.y = 0.0; @@ -491,7 +488,7 @@ visualization_msgs::Marker convertToMarker( marker.color = color; - marker.lifetime = ros::Duration(1); + marker.lifetime = rclcpp::Duration::from_seconds(1.0); marker.frame_locked = true; for (std::size_t i = 0; i < path.points.size(); i++) { @@ -501,19 +498,19 @@ visualization_msgs::Marker convertToMarker( return marker; } -visualization_msgs::MarkerArray createVirtualWall( - const geometry_msgs::Pose & pose, const int id, const std::string & factor_text) +visualization_msgs::msg::MarkerArray createVirtualWall( + const geometry_msgs::msg::Pose & pose, const int id, const std::string & factor_text, const rclcpp::Time & timestamp) { - visualization_msgs::MarkerArray msg; + visualization_msgs::msg::MarkerArray msg; { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = timestamp; marker.ns = "stop_virtual_wall"; marker.id = id; - marker.lifetime = ros::Duration(0.5); - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = pose; marker.pose.position.z += 1.0; marker.scale.x = 0.1; @@ -527,14 +524,14 @@ visualization_msgs::MarkerArray createVirtualWall( } // Factor Text { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = timestamp; marker.ns = "factor_text"; marker.id = id; - marker.lifetime = ros::Duration(0.5); - marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; marker.pose = pose; marker.pose.position.z += 2.0; marker.scale.x = 0.0; diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/route_handler.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/route_handler.cpp index 3ea5a54163aba..9e8fc468c7577 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/route_handler.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/route_handler.cpp @@ -17,22 +17,22 @@ #include #include -#include #include #include #include #include #include #include +#include -#include +#include #include #include #include -using autoware_planning_msgs::PathPointWithLaneId; -using autoware_planning_msgs::PathWithLaneId; +using autoware_planning_msgs::msg::PathPointWithLaneId; +using autoware_planning_msgs::msg::PathWithLaneId; using lanelet::utils::to2D; namespace { @@ -47,7 +47,7 @@ bool exists(const std::vector & vectors, const T & item) return false; } -bool isRouteLooped(const autoware_planning_msgs::Route & route_msg) +bool isRouteLooped(const autoware_planning_msgs::msg::Route & route_msg) { const auto & route_sections = route_msg.route_sections; for (std::size_t i = 0; i < route_sections.size(); i++) { @@ -66,7 +66,7 @@ bool isRouteLooped(const autoware_planning_msgs::Route & route_msg) PathWithLaneId combineReferencePath( const PathWithLaneId path1, const PathWithLaneId path2, const double interval, - const size_t N_sample) + const size_t N_sample, const rclcpp::Logger & logger) { PathWithLaneId path; path.points.insert(path.points.end(), path1.points.begin(), path1.points.end()); @@ -118,7 +118,7 @@ PathWithLaneId combineReferencePath( inner_s.push_back(d); } - lane_change_planner::util::SplineInterpolate spline; + lane_change_planner::util::SplineInterpolate spline(logger); std::vector inner_points; std::vector inner_x; std::vector inner_y; @@ -146,7 +146,7 @@ PathWithLaneId combineReferencePath( //set yaw for (size_t i = 0; i < inner_points.size(); ++i) { - geometry_msgs::Point prev, next; + geometry_msgs::msg::Point prev, next; if (i == 0) { prev = path1.points.back().point.pose.position; } else { @@ -167,7 +167,8 @@ PathWithLaneId combineReferencePath( path.points.insert(path.points.end(), inner_points.begin(), inner_points.end()); } else { - ROS_WARN("[LaneChageModule::splineInterpolate] spline interpolation failed."); + RCLCPP_WARN( + logger, "[LaneChageModule::splineInterpolate] spline interpolation failed."); } } path.points.insert(path.points.end(), path2.points.begin(), path2.points.end()); @@ -203,16 +204,20 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( namespace lane_change_planner { -RouteHandler::RouteHandler() -: is_route_msg_ready_(false), is_map_msg_ready_(false), is_handler_ready_(false) +RouteHandler::RouteHandler(const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) +: is_map_msg_ready_(false), + is_route_msg_ready_(false), + is_handler_ready_(false), + logger_(logger), + clock_(clock) { } -void RouteHandler::mapCallback(const autoware_lanelet2_msgs::MapBin & map_msg) +void RouteHandler::mapCallback(const autoware_lanelet2_msgs::msg::MapBin::ConstSharedPtr map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( - map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); + *map_msg, lanelet_map_ptr_, &traffic_rules_ptr_, &routing_graph_ptr_); const auto traffic_rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); @@ -232,15 +237,16 @@ void RouteHandler::mapCallback(const autoware_lanelet2_msgs::MapBin & map_msg) setRouteLanelets(); } -void RouteHandler::routeCallback(const autoware_planning_msgs::Route & route_msg) +void RouteHandler::routeCallback(const autoware_planning_msgs::msg::Route::ConstSharedPtr route_msg) { - if (!isRouteLooped(route_msg)) { - route_msg_ = route_msg; + if (!isRouteLooped(*route_msg)) { + route_msg_ = *route_msg; is_route_msg_ready_ = true; is_handler_ready_ = false; setRouteLanelets(); } else { - ROS_ERROR( + RCLCPP_ERROR( + logger_, "Loop detected within route! Currently, no loop is allowed for route! Using previous route"); } } @@ -298,7 +304,7 @@ std::vector RouteHandler::getLanesAfterGoal( lanelet::ConstLanelets RouteHandler::getRouteLanelets() const { return route_lanelets_; } -geometry_msgs::Pose RouteHandler::getGoalPose() const { return route_msg_.goal_pose; } +geometry_msgs::msg::Pose RouteHandler::getGoalPose() const { return route_msg_.goal_pose; } lanelet::Id RouteHandler::getGoalLaneId() const { @@ -359,7 +365,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( double length = 0; lanelet::ConstLanelet current_lanelet = lanelet; - while (ros::ok() && length < min_length) { + while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { break; @@ -383,7 +389,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; - while (ros::ok() && length < min_length) { + while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet prev_lanelet; if (!getPreviousLaneletWithinRoute(current_lanelet, &prev_lanelet)) { break; @@ -399,7 +405,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelets RouteHandler::getLaneletSequence(const lanelet::ConstLanelet & lanelet) const { - geometry_msgs::Pose tmp_pose; + geometry_msgs::msg::Pose tmp_pose; tmp_pose.orientation.w = 1; if (!lanelet.centerline().empty()) { tmp_pose.position = lanelet::utils::conversion::toGeomMsgPt(lanelet.centerline().front()); @@ -409,7 +415,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence(const lanelet::ConstLane } lanelet::ConstLanelets RouteHandler::getLaneletSequence( - const lanelet::ConstLanelet & lanelet, const geometry_msgs::Pose & current_pose, + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Pose & current_pose, const double backward_distance, const double forward_distance) const { lanelet::ConstLanelets lanelet_sequence; @@ -427,7 +433,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( } // loop check - if (lanelet_sequence_forward.empty() > 1 && lanelet_sequence_backward.empty() > 1) { + if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) { if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) { return lanelet_sequence_forward; } @@ -442,7 +448,7 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( } bool RouteHandler::getClosestLaneletWithinRoute( - const geometry_msgs::Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const + const geometry_msgs::msg::Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const { return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } @@ -527,7 +533,7 @@ bool RouteHandler::getLaneChangeTarget( } lanelet::ConstLanelets RouteHandler::getClosestLaneletSequence( - const geometry_msgs::Pose & pose) const + const geometry_msgs::msg::Pose & pose) const { lanelet::ConstLanelet lanelet; lanelet::ConstLanelets empty_lanelets; @@ -559,9 +565,10 @@ int RouteHandler::getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanele return num; } } + return 0; } -bool RouteHandler::isInPreferredLane(const geometry_msgs::PoseStamped & pose) const +bool RouteHandler::isInPreferredLane(const geometry_msgs::msg::PoseStamped & pose) const { lanelet::ConstLanelet lanelet; if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { @@ -570,7 +577,7 @@ bool RouteHandler::isInPreferredLane(const geometry_msgs::PoseStamped & pose) co return exists(preferred_lanelets_, lanelet); } bool RouteHandler::isInTargetLane( - const geometry_msgs::PoseStamped & pose, const lanelet::ConstLanelets & target) const + const geometry_msgs::msg::PoseStamped & pose, const lanelet::ConstLanelets & target) const { lanelet::ConstLanelet lanelet; if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { @@ -580,7 +587,7 @@ bool RouteHandler::isInTargetLane( } PathWithLaneId RouteHandler::getReferencePath( - const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::Pose & pose, + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose, const double backward_path_length, const double forward_path_length, const double minimum_lane_change_length) const { @@ -711,7 +718,8 @@ PathWithLaneId RouteHandler::updatePathTwist(const PathWithLaneId & path) const return updated_path; } -lanelet::ConstLanelets RouteHandler::getLaneChangeTarget(const geometry_msgs::Pose & pose) const +lanelet::ConstLanelets RouteHandler::getLaneChangeTarget( + const geometry_msgs::msg::Pose & pose) const { lanelet::ConstLanelet lanelet; lanelet::ConstLanelets target_lanelets; @@ -737,7 +745,7 @@ lanelet::ConstLanelets RouteHandler::getLaneChangeTarget(const geometry_msgs::Po std::vector RouteHandler::getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist, const LaneChangerParameters & parameter) const { std::vector candidate_paths; @@ -779,7 +787,6 @@ std::vector RouteHandler::getLaneChangePaths( PathWithLaneId reference_path1; { - const double lane_length = lanelet::utils::getLaneletLength2d(original_lanelets); const auto arc_position = lanelet::utils::getArcCoordinates(original_lanelets, pose); const double s_start = arc_position.length - backward_path_length; const double s_end = arc_position.length + straight_distance; @@ -800,7 +807,7 @@ std::vector RouteHandler::getLaneChangePaths( } if (reference_path1.points.empty() || reference_path2.points.empty()) { - ROS_ERROR_STREAM("reference path is empty!! something wrong..."); + RCLCPP_ERROR_STREAM(logger_, "reference path is empty!! something wrong..."); continue; } @@ -808,11 +815,11 @@ std::vector RouteHandler::getLaneChangePaths( candidate_path.acceleration = acceleration; candidate_path.preparation_length = straight_distance; candidate_path.lane_change_length = lane_change_distance; - candidate_path.path = combineReferencePath(reference_path1, reference_path2, 5.0, 2); + candidate_path.path = combineReferencePath(reference_path1, reference_path2, 5.0, 2, logger_); // set fixed flag for (auto & pt : candidate_path.path.points) { - pt.point.type = autoware_planning_msgs::PathPoint::FIXED; + pt.point.type = autoware_planning_msgs::msg::PathPoint::FIXED; } candidate_paths.push_back(candidate_path); } @@ -821,7 +828,7 @@ std::vector RouteHandler::getLaneChangePaths( } double RouteHandler::getLaneChangeableDistance( - const geometry_msgs::Pose & current_pose, const LaneChangeDirection & direction) + const geometry_msgs::msg::Pose & current_pose, const LaneChangeDirection & direction) { lanelet::ConstLanelet current_lane; if (!getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -870,8 +877,8 @@ double RouteHandler::getLaneChangeableDistance( } lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( - const autoware_planning_msgs::PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, - const double check_length) + const autoware_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & target_lanes, const double check_length) { std::vector target_lane_ids; target_lane_ids.reserve(target_lanes.size()); diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/aborting_lane_change.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/aborting_lane_change.cpp index b4175114e5a96..aa8e5753a4d67 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/aborting_lane_change.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/aborting_lane_change.cpp @@ -28,7 +28,7 @@ State AbortingLaneChangeState::getCurrentState() const { return State::ABORTING_ void AbortingLaneChangeState::entry() {} -autoware_planning_msgs::PathWithLaneId AbortingLaneChangeState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId AbortingLaneChangeState::getPath() const { return status_.lane_follow_path; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/blocked_by_obstacle.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/blocked_by_obstacle.cpp index 2f0d7d5ab8ee7..0ed2fa3acd794 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/blocked_by_obstacle.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/blocked_by_obstacle.cpp @@ -43,7 +43,7 @@ void BlockedByObstacleState::entry() current_lanes_ = route_handler_ptr_->getLaneletsFromIds(status_.lane_follow_lane_ids); } -autoware_planning_msgs::PathWithLaneId BlockedByObstacleState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId BlockedByObstacleState::getPath() const { return status_.lane_follow_path; } @@ -65,7 +65,7 @@ void BlockedByObstacleState::update() // update lanes { if (!route_handler_ptr_->getClosestLaneletWithinRoute(current_pose_.pose, ¤t_lane)) { - ROS_ERROR("failed to find closest lanelet within route!!!"); + RCLCPP_ERROR(data_manager_ptr_->getLogger(), "failed to find closest lanelet within route!!!"); return; } lanelet::ConstLanelet right_lane; @@ -82,8 +82,6 @@ void BlockedByObstacleState::update() } const double minimum_lane_change_length = ros_parameters_.minimum_lane_change_length; - const double lane_change_prepare_duration = ros_parameters_.lane_change_prepare_duration; - const double lane_changing_duration = ros_parameters_.lane_changing_duration; // update lane_follow_path { @@ -134,7 +132,7 @@ void BlockedByObstacleState::update() lane_change_paths, current_lanes_, check_lanes, route_handler_ptr_->getOverallGraph(), dynamic_objects_, current_pose_.pose, current_twist_->twist, route_handler_ptr_->isInGoalRouteSection(current_lanes_.back()), - route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path)) { + route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path, data_manager_ptr_->getLogger(), data_manager_ptr_->getClock())) { found_safe_path_ = true; } debug_data_.selected_path = selected_path.path; @@ -165,7 +163,8 @@ void BlockedByObstacleState::update() lane_change_paths, current_lanes_, check_lanes, route_handler_ptr_->getOverallGraph(), dynamic_objects_, current_pose_.pose, current_twist_->twist, route_handler_ptr_->isInGoalRouteSection(current_lanes_.back()), - route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path)) { + route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path, + data_manager_ptr_->getLogger(), data_manager_ptr_->getClock())) { found_safe_path_ = true; } @@ -203,8 +202,8 @@ State BlockedByObstacleState::getNextState() const return State::BLOCKED_BY_OBSTACLE; } -autoware_planning_msgs::PathWithLaneId BlockedByObstacleState::setStopPointFromObstacle( - const autoware_planning_msgs::PathWithLaneId & path) +autoware_planning_msgs::msg::PathWithLaneId BlockedByObstacleState::setStopPointFromObstacle( + const autoware_planning_msgs::msg::PathWithLaneId & path) { const auto blocking_objects = getBlockingObstacles(); @@ -217,7 +216,7 @@ autoware_planning_msgs::PathWithLaneId BlockedByObstacleState::setStopPointFromO } // find the closest static obstacle in front of ego vehicle - autoware_perception_msgs::DynamicObject closest_object; + autoware_perception_msgs::msg::DynamicObject closest_object; bool found_closest_object = false; double closest_distance = std::numeric_limits::max(); for (const auto & object : blocking_objects) { @@ -240,7 +239,7 @@ autoware_planning_msgs::PathWithLaneId BlockedByObstacleState::setStopPointFromO double stop_insert_length = closest_distance - ros_parameters_.minimum_lane_change_length - ros_parameters_.base_link2front; stop_insert_length = std::max(stop_insert_length, 0.0); - autoware_planning_msgs::PathWithLaneId modified_path = path; + autoware_planning_msgs::msg::PathWithLaneId modified_path = path; debug_data_.stop_factor_point = closest_object.state.pose_covariance.pose.position; debug_data_.stop_point = util::insertStopPoint(stop_insert_length, &modified_path); return modified_path; @@ -250,7 +249,7 @@ bool BlockedByObstacleState::isOutOfCurrentLanes() const { lanelet::ConstLanelet closest_lane; if (!route_handler_ptr_->getClosestLaneletWithinRoute(current_pose_.pose, &closest_lane)) { - ROS_ERROR("failed to find closest lanelet within route!!!"); + RCLCPP_ERROR(data_manager_ptr_->getLogger(), "failed to find closest lanelet within route!!!"); return true; } for (const auto & llt : current_lanes_) { @@ -267,10 +266,10 @@ bool BlockedByObstacleState::isLaneBlocked() const return !blocking_objects.empty(); } -std::vector BlockedByObstacleState::getBlockingObstacles() - const +std::vector +BlockedByObstacleState::getBlockingObstacles() const { - std::vector blocking_obstacles; + std::vector blocking_obstacles; const auto arc = lanelet::utils::getArcCoordinates(current_lanes_, current_pose_.pose); constexpr double max_check_distance = 100; @@ -286,9 +285,9 @@ std::vector BlockedByObstacleState::get current_lanes_, arc.length, arc.length + check_distance); if (polygon.size() < 3) { - ROS_WARN_STREAM( - "could not get polygon from lanelet with arc lengths: " << arc.length << " to " - << arc.length + check_distance); + RCLCPP_WARN_STREAM( + data_manager_ptr_->getLogger(), "could not get polygon from lanelet with arc lengths: " + << arc.length << " to " << arc.length + check_distance); return blocking_obstacles; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/common_functions.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/common_functions.cpp index 8d1a567004d97..4b31b87d6699f 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/common_functions.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/common_functions.cpp @@ -27,15 +27,15 @@ bool selectLaneChangePath( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::routing::RoutingGraphContainer & overall_graphs, - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & dynamic_objects, - const geometry_msgs::Pose & current_pose, const geometry_msgs::Twist & current_twist, - const bool isInGoalRouteSection, const geometry_msgs::Pose & goal_pose, - const LaneChangerParameters & ros_parameters, LaneChangePath * selected_path) + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr & dynamic_objects, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Twist & current_twist, + const bool isInGoalRouteSection, const geometry_msgs::msg::Pose & goal_pose, + const LaneChangerParameters & ros_parameters, LaneChangePath * selected_path, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) { for (const auto & path : paths) { if (!isLaneChangePathSafe( path.path, current_lanes, target_lanes, dynamic_objects, current_pose, current_twist, - ros_parameters, true, path.acceleration)) { + ros_parameters, logger, clock, true, path.acceleration)) { continue; } if (!hasEnoughDistance( @@ -58,8 +58,8 @@ bool selectLaneChangePath( bool hasEnoughDistance( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const geometry_msgs::Pose & current_pose, - const bool isInGoalRouteSection, const geometry_msgs::Pose & goal_pose, + const lanelet::ConstLanelets & target_lanes, const geometry_msgs::msg::Pose & current_pose, + const bool isInGoalRouteSection, const geometry_msgs::msg::Pose & goal_pose, const lanelet::routing::RoutingGraphContainer & overall_graphs) { const double lane_change_prepare_distance = path.preparation_length; @@ -90,11 +90,11 @@ bool hasEnoughDistance( } bool isLaneChangePathSafe( - const autoware_planning_msgs::PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const autoware_perception_msgs::DynamicObjectArray::ConstPtr & dynamic_objects, - const geometry_msgs::Pose & current_pose, const geometry_msgs::Twist & current_twist, - const LaneChangerParameters & ros_parameters, const bool use_buffer, const double acceleration) + const autoware_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr & dynamic_objects, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Twist & current_twist, + const LaneChangerParameters & ros_parameters, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock, const bool use_buffer, const double acceleration) { if (path.points.empty()) { return false; @@ -110,10 +110,6 @@ bool isLaneChangePathSafe( // parameters const double time_resolution = ros_parameters.prediction_time_resolution; - const double prediction_duration = ros_parameters.prediction_duration; - const double current_lane_check_start_time = 0.0; - const double current_lane_check_end_time = - ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; const double target_lane_check_start_time = 0.0; const double target_lane_check_end_time = ros_parameters.lane_change_prepare_duration + ros_parameters.lane_changing_duration; @@ -133,24 +129,24 @@ bool isLaneChangePathSafe( // find obstacle in lane change target lanes // retrieve lanes that are merging target lanes as well const auto target_lane_object_indices = - util::filterObjectsByLanelets(*dynamic_objects, target_lanes); + util::filterObjectsByLanelets(*dynamic_objects, target_lanes, logger); // find objects in current lane const auto current_lane_object_indices_lanelet = util::filterObjectsByLanelets( - *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); + *dynamic_objects, current_lanes, arc.length, arc.length + check_distance, logger); const auto current_lane_object_indices = util::filterObjectsByPath( *dynamic_objects, current_lane_object_indices_lanelet, path, - vehicle_width / 2 + lateral_buffer); + vehicle_width / 2 + lateral_buffer, logger); const auto & vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration); + path, current_twist, current_pose, target_lane_check_end_time, time_resolution, acceleration, logger, clock); for (const auto & i : current_lane_object_indices) { const auto & obj = dynamic_objects->objects.at(i); for (const auto & obj_path : obj.state.predicted_paths) { double distance = util::getDistanceBetweenPredictedPaths( obj_path, vehicle_predicted_path, target_lane_check_start_time, target_lane_check_end_time, - time_resolution); + time_resolution, logger, clock); double thresh; if (isObjectFront(current_pose, obj.state.pose_covariance.pose)) { thresh = util::l2Norm(current_twist.linear) * stop_time; @@ -170,7 +166,7 @@ bool isLaneChangePathSafe( for (const auto & obj_path : obj.state.predicted_paths) { double distance = util::getDistanceBetweenPredictedPaths( obj_path, vehicle_predicted_path, target_lane_check_start_time, target_lane_check_end_time, - time_resolution); + time_resolution, logger, clock); double thresh; if (isObjectFront(current_pose, obj.state.pose_covariance.pose)) { thresh = util::l2Norm(current_twist.linear) * stop_time; @@ -188,10 +184,11 @@ bool isLaneChangePathSafe( return true; } -bool isObjectFront(const geometry_msgs::Pose & ego_pose, const geometry_msgs::Pose & obj_pose) +bool isObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const geometry_msgs::msg::Pose & obj_pose) { tf2::Transform tf_map2ego, tf_map2obj; - geometry_msgs::Pose obj_from_ego; + geometry_msgs::msg::Pose obj_from_ego; tf2::fromMsg(ego_pose, tf_map2ego); tf2::fromMsg(obj_pose, tf_map2obj); tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/executing_lane_change.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/executing_lane_change.cpp index b41e78f8f60e9..5383098871c9c 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/executing_lane_change.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/executing_lane_change.cpp @@ -44,7 +44,7 @@ void ExecutingLaneChangeState::entry() status_.lane_change_ready = false; } -autoware_planning_msgs::PathWithLaneId ExecutingLaneChangeState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId ExecutingLaneChangeState::getPath() const { return status_.lane_change_path.path; } @@ -112,8 +112,8 @@ bool ExecutingLaneChangeState::isAbortConditionSatisfied() const lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( original_lanes_, current_pose_.pose, &closest_lanelet)) { - ROS_ERROR_THROTTLE( - 1, "Failed to find closest lane! Lane change aborting function is not working!"); + RCLCPP_ERROR_THROTTLE(data_manager_ptr_->getLogger(), *data_manager_ptr_->getClock(), + 1.0, "Failed to find closest lane! Lane change aborting function is not working!"); return false; } @@ -130,7 +130,7 @@ bool ExecutingLaneChangeState::isAbortConditionSatisfied() const is_path_safe = state_machine::common_functions::isLaneChangePathSafe( path.path, original_lanes_, check_lanes, dynamic_objects_, current_pose_.pose, - current_twist_->twist, ros_parameters_, false, status_.lane_change_path.acceleration); + current_twist_->twist, ros_parameters_, data_manager_ptr_->getLogger(), data_manager_ptr_->getClock(), false, status_.lane_change_path.acceleration); } // check vehicle velocity thresh @@ -177,8 +177,8 @@ bool ExecutingLaneChangeState::isAbortConditionSatisfied() const if (is_distance_small && is_angle_diff_small) { return true; } - ROS_WARN_STREAM_THROTTLE( - 1, "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be catious"); + RCLCPP_WARN_STREAM_THROTTLE(data_manager_ptr_->getLogger(), *data_manager_ptr_->getClock(), + 1.0, "DANGER!!! Path is not safe anymore, but it is too late to abort! Please be catious"); } return false; @@ -186,12 +186,13 @@ bool ExecutingLaneChangeState::isAbortConditionSatisfied() const bool ExecutingLaneChangeState::hasFinishedLaneChange() const { - static ros::Time start_time = ros::Time::now(); + const auto & clock = data_manager_ptr_->getClock(); + static rclcpp::Time start_time = clock->now(); if (route_handler_ptr_->isInTargetLane(current_pose_, target_lanes_)) { - return (ros::Time::now() - start_time > ros::Duration(2)); + return (clock->now() - start_time > rclcpp::Duration::from_seconds(2.0)); } else { - start_time = ros::Time::now(); + start_time = clock->now(); } return false; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/following_lane.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/following_lane.cpp index 3bfe5bb755625..c7ade40b80cc0 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/following_lane.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/following_lane.cpp @@ -42,7 +42,7 @@ void FollowingLaneState::entry() status_.lane_change_ready = false; } -autoware_planning_msgs::PathWithLaneId FollowingLaneState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId FollowingLaneState::getPath() const { return status_.lane_follow_path; } @@ -64,7 +64,7 @@ void FollowingLaneState::update() // update lanes { if (!route_handler_ptr_->getClosestLaneletWithinRoute(current_pose_.pose, ¤t_lane)) { - ROS_ERROR("failed to find closest lanelet within route!!!"); + RCLCPP_ERROR(data_manager_ptr_->getLogger(), "failed to find closest lanelet within route!!!"); return; } current_lanes_ = route_handler_ptr_->getLaneletSequence( @@ -81,8 +81,6 @@ void FollowingLaneState::update() // update lane_follow_path { constexpr double check_distance = 100.0; - const double lane_change_prepare_duration = ros_parameters_.lane_change_prepare_duration; - const double lane_changing_duration = ros_parameters_.lane_changing_duration; const double minimum_lane_change_length = ros_parameters_.minimum_lane_change_length; status_.lane_follow_path = route_handler_ptr_->getReferencePath( current_lanes_, current_pose_.pose, backward_path_length, forward_path_length, @@ -112,7 +110,7 @@ void FollowingLaneState::update() lane_change_paths, current_lanes_, check_lanes, route_handler_ptr_->getOverallGraph(), dynamic_objects_, current_pose_.pose, current_twist_->twist, route_handler_ptr_->isInGoalRouteSection(current_lanes_.back()), - route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path)) { + route_handler_ptr_->getGoalPose(), ros_parameters_, &selected_path, data_manager_ptr_->getLogger(), data_manager_ptr_->getClock())) { found_safe_path = true; } debug_data_.selected_path = selected_path.path; @@ -149,7 +147,7 @@ void FollowingLaneState::update() State FollowingLaneState::getNextState() const { if (current_lanes_.empty()) { - ROS_ERROR_THROTTLE(1, "current lanes empty. Keeping state."); + RCLCPP_ERROR_THROTTLE(data_manager_ptr_->getLogger(), *data_manager_ptr_->getClock(), 1.0, "current lanes empty. Keeping state."); return State::FOLLOWING_LANE; } if (route_handler_ptr_->isInPreferredLane(current_pose_) && isLaneBlocked(current_lanes_)) { @@ -180,7 +178,7 @@ bool FollowingLaneState::isLaneBlocked(const lanelet::ConstLanelets & lanes) con lanelet::utils::getPolygonFromArcLength(lanes, arc.length, arc.length + check_distance); if (polygon.size() < 3) { - ROS_WARN_STREAM( + RCLCPP_WARN_STREAM(data_manager_ptr_->getLogger(), "could not get polygon from lanelet with arc lengths: " << arc.length << " to " << arc.length + check_distance); return false; @@ -188,10 +186,10 @@ bool FollowingLaneState::isLaneBlocked(const lanelet::ConstLanelets & lanes) con for (const auto & obj : dynamic_objects_->objects) { if ( - obj.semantic.type == autoware_perception_msgs::Semantic::CAR || - obj.semantic.type == autoware_perception_msgs::Semantic::TRUCK || - obj.semantic.type == autoware_perception_msgs::Semantic::BUS || - obj.semantic.type == autoware_perception_msgs::Semantic::MOTORBIKE) { + obj.semantic.type == autoware_perception_msgs::msg::Semantic::CAR || + obj.semantic.type == autoware_perception_msgs::msg::Semantic::TRUCK || + obj.semantic.type == autoware_perception_msgs::msg::Semantic::BUS || + obj.semantic.type == autoware_perception_msgs::msg::Semantic::MOTORBIKE) { const auto velocity = util::l2Norm(obj.state.twist_covariance.twist.linear); if (velocity < static_obj_velocity_thresh) { const auto position = diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/forcing_lane_change.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/forcing_lane_change.cpp index d2b4f0c44f4fe..0589218383ed9 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/forcing_lane_change.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/forcing_lane_change.cpp @@ -37,7 +37,7 @@ void ForcingLaneChangeState::entry() target_lanes_ = route_handler_ptr_->getLaneletsFromIds(status_.lane_change_lane_ids); } -autoware_planning_msgs::PathWithLaneId ForcingLaneChangeState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId ForcingLaneChangeState::getPath() const { return status_.lane_change_path.path; } @@ -71,12 +71,13 @@ State ForcingLaneChangeState::getNextState() const bool ForcingLaneChangeState::hasFinishedLaneChange() const { - static ros::Time start_time = ros::Time::now(); + const auto & clock = data_manager_ptr_->getClock(); + static rclcpp::Time start_time = clock->now(); if (route_handler_ptr_->isInTargetLane(current_pose_, target_lanes_)) { - return (ros::Time::now() - start_time > ros::Duration(2)); + return (clock->now() - start_time > rclcpp::Duration::from_seconds(2.0)); } else { - start_time = ros::Time::now(); + start_time = clock->now(); } return false; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/state_base_class.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/state_base_class.cpp index de1c500c35c85..60a2d28d2b4c7 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/state_base_class.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/state_base_class.cpp @@ -52,7 +52,7 @@ std::ostream & operator<<(std::ostream & ostream, const State & state) StateBase::StateBase( const Status & status, const std::shared_ptr & data_manager_ptr, const std::shared_ptr & route_handler_ptr) -: data_manager_ptr_(data_manager_ptr), route_handler_ptr_(route_handler_ptr), status_(status) +: status_(status), data_manager_ptr_(data_manager_ptr), route_handler_ptr_(route_handler_ptr) { } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/stopping_lane_change.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/stopping_lane_change.cpp index f816c722550da..7d22912758359 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/stopping_lane_change.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state/stopping_lane_change.cpp @@ -41,7 +41,7 @@ void StoppingLaneChangeState::entry() status_.lane_change_ready = false; } -autoware_planning_msgs::PathWithLaneId StoppingLaneChangeState::getPath() const +autoware_planning_msgs::msg::PathWithLaneId StoppingLaneChangeState::getPath() const { return status_.lane_change_path.path; } @@ -77,8 +77,8 @@ bool StoppingLaneChangeState::isSafe() const is_path_safe = state_machine::common_functions::isLaneChangePathSafe( status_.lane_change_path.path, original_lanes_, check_lanes, dynamic_objects_, - current_pose_.pose, current_twist_->twist, ros_parameters_, false, - status_.lane_change_path.acceleration); + current_pose_.pose, current_twist_->twist, ros_parameters_, data_manager_ptr_->getLogger(), + data_manager_ptr_->getClock(), false, status_.lane_change_path.acceleration); } return is_path_safe; } @@ -102,10 +102,10 @@ bool StoppingLaneChangeState::isVehicleInOriginalLanes() const return intersection_area / vehicle_area > 0.9; } -autoware_planning_msgs::PathWithLaneId StoppingLaneChangeState::setStopPoint( - const autoware_planning_msgs::PathWithLaneId & path) +autoware_planning_msgs::msg::PathWithLaneId StoppingLaneChangeState::setStopPoint( + const autoware_planning_msgs::msg::PathWithLaneId & path) { - autoware_planning_msgs::PathWithLaneId modified_path = path; + autoware_planning_msgs::msg::PathWithLaneId modified_path = path; debug_data_.stop_point = util::insertStopPoint(0.1, &modified_path); return modified_path; } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state_machine.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state_machine.cpp index c7b004ed1d3bb..43ada404a7259 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state_machine.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/state_machine.cpp @@ -21,9 +21,9 @@ #include #include #include -#include +#include -#include +#include namespace lane_change_planner { @@ -42,7 +42,7 @@ void StateMachine::init() state_obj_ptr_->entry(); } -void StateMachine::init(const autoware_planning_msgs::Route & route) { init(); } +void StateMachine::initCallback(const autoware_planning_msgs::msg::Route::ConstSharedPtr route) { init(); } void StateMachine::updateState() { @@ -53,7 +53,7 @@ void StateMachine::updateState() // Transit to next state if (next_state != current_state) { - ROS_INFO_STREAM("changing state: " << current_state << " => " << next_state); + RCLCPP_INFO_STREAM(data_manager_ptr_->getLogger(), "changing state: " << current_state << " => " << next_state); const auto previous_status = state_obj_ptr_->getStatus(); switch (next_state) { case State::FOLLOWING_LANE: @@ -80,13 +80,15 @@ void StateMachine::updateState() state_obj_ptr_ = std::make_unique( previous_status, data_manager_ptr_, route_handler_ptr_); break; + default: + break; } state_obj_ptr_->entry(); state_obj_ptr_->update(); } } -autoware_planning_msgs::PathWithLaneId StateMachine::getPath() const +autoware_planning_msgs::msg::PathWithLaneId StateMachine::getPath() const { return state_obj_ptr_->getPath(); } diff --git a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/utilities.cpp b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/utilities.cpp index 4dc7a35b7bff4..9c7a90c97a565 100644 --- a/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/utilities.cpp +++ b/planning/scenario_planning/lane_driving/behavior_planning/lane_change_planner/src/utilities.cpp @@ -23,33 +23,33 @@ namespace { -ros::Duration safeSubtraction(const ros::Time & t1, const ros::Time & t2) +rclcpp::Duration safeSubtraction(const rclcpp::Time & t1, const rclcpp::Time & t2) { - ros::Duration duration; + rclcpp::Duration duration(0, 0); try { duration = t1 - t2; - } catch (std::runtime_error) { + } catch (std::runtime_error & err) { if (t1 > t2) - duration = ros::DURATION_MIN; + duration = rclcpp::Duration::max() * -1.0; else - duration = ros::DURATION_MAX; + duration = rclcpp::Duration::max(); } return duration; } -ros::Time safeAddition(const ros::Time & t1, const double seconds) +rclcpp::Time safeAddition(const rclcpp::Time & t1, const double seconds) { - ros::Time sum; + rclcpp::Time sum; try { - sum = t1 + ros::Duration(seconds); + sum = t1 + rclcpp::Duration::from_seconds(seconds); } catch (std::runtime_error & err) { - if (seconds > 0) sum = ros::TIME_MAX; - if (seconds < 0) sum = ros::TIME_MIN; + if (seconds > 0) sum = rclcpp::Time::max(); + if (seconds < 0) sum = rclcpp::Time(0); } return sum; } cv::Point toCVPoint( - const geometry_msgs::Point & geom_point, const double width_m, const double height_m, + const geometry_msgs::msg::Point & geom_point, const double width_m, const double height_m, const double resolution) { return cv::Point( @@ -57,7 +57,7 @@ cv::Point toCVPoint( static_cast((width_m - geom_point.x) / resolution)); } -void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::OccupancyGrid * occupancy_grid) +void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid) { occupancy_grid->data.reserve(cv_image.rows * cv_image.cols); for (int x = cv_image.cols - 1; x >= 0; x--) { @@ -68,14 +68,25 @@ void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::OccupancyGrid * oc } } +geometry_msgs::msg::TransformStamped toTransformStamped(const geometry_msgs::msg::PoseStamped pose) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header = pose.header; + transform.transform.translation.x = pose.pose.position.x; + transform.transform.translation.y = pose.pose.position.y; + transform.transform.translation.z = pose.pose.position.z; + transform.transform.rotation = pose.pose.orientation; + return transform; +} + } // namespace namespace lane_change_planner { namespace util { -using autoware_perception_msgs::PredictedPath; -using autoware_planning_msgs::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_planning_msgs::msg::PathWithLaneId; double normalizeRadian(const double radian) { @@ -89,20 +100,20 @@ double normalizeRadian(const double radian) return normalized; } -double l2Norm(const geometry_msgs::Vector3 vector) +double l2Norm(const geometry_msgs::msg::Vector3 vector) { return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); } -Eigen::Vector3d convertToEigenPt(const geometry_msgs::Point geom_pt) +Eigen::Vector3d convertToEigenPt(const geometry_msgs::msg::Point geom_pt) { return Eigen::Vector3d(geom_pt.x, geom_pt.y, geom_pt.z); } // returns false when search point is off the linestring bool convertToFrenetCoordinate3d( - const std::vector & linestring, - const geometry_msgs::Point search_point_geom, FrenetCoordinate3d * frenet_coordinate) + const std::vector & linestring, + const geometry_msgs::msg::Point search_point_geom, FrenetCoordinate3d * frenet_coordinate) { if (linestring.empty()) { return false; @@ -169,9 +180,9 @@ bool convertToFrenetCoordinate3d( return found; } -std::vector convertToGeometryPointArray(const PathWithLaneId & path) +std::vector convertToGeometryPointArray(const PathWithLaneId & path) { - std::vector converted_path; + std::vector converted_path; converted_path.reserve(path.points.size()); for (const auto & point_with_id : path.points) { converted_path.push_back(point_with_id.point.pose.position); @@ -179,9 +190,9 @@ std::vector convertToGeometryPointArray(const PathWithLane return converted_path; } -std::vector convertToGeometryPointArray(const PredictedPath & path) +std::vector convertToGeometryPointArray(const PredictedPath & path) { - std::vector converted_path; + std::vector converted_path; converted_path.reserve(path.path.size()); for (const auto & pose_with_cov_stamped : path.path) { @@ -190,9 +201,9 @@ std::vector convertToGeometryPointArray(const PredictedPat return converted_path; } -geometry_msgs::PoseArray convertToGeometryPoseArray(const PathWithLaneId & path) +geometry_msgs::msg::PoseArray convertToGeometryPoseArray(const PathWithLaneId & path) { - geometry_msgs::PoseArray converted_array; + geometry_msgs::msg::PoseArray converted_array; converted_array.header = path.header; converted_array.poses.reserve(path.points.size()); @@ -203,9 +214,9 @@ geometry_msgs::PoseArray convertToGeometryPoseArray(const PathWithLaneId & path) } PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const geometry_msgs::Twist & vehicle_twist, - const geometry_msgs::Pose & vehicle_pose, const double duration, const double resolution, - const double acceleration) + const PathWithLaneId & path, const geometry_msgs::msg::Twist & vehicle_twist, + const geometry_msgs::msg::Pose & vehicle_pose, const double duration, const double resolution, + const double acceleration, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) { PredictedPath predicted_path; predicted_path.path.reserve(path.points.size()); @@ -216,14 +227,15 @@ PredictedPath convertToPredictedPath( const auto & geometry_points = convertToGeometryPointArray(path); FrenetCoordinate3d vehicle_pose_frenet; convertToFrenetCoordinate3d(geometry_points, vehicle_pose.position, &vehicle_pose_frenet); - ros::Time start_time = ros::Time::now(); + rclcpp::Time start_time = clock->now(); double vehicle_speed = std::abs(vehicle_twist.linear.x); constexpr double min_speed = 1.0; if (vehicle_speed < min_speed) { vehicle_speed = min_speed; - ROS_DEBUG_STREAM_THROTTLE( - 1, "cannot convert PathWithLaneId with zero velocity, using minimum value " - << min_speed << " [m/s] instead"); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger, *clock, 1.0, + "cannot convert PathWithLaneId with zero velocity, using minimum value " << min_speed + << " [m/s] instead"); } double length = 0; @@ -231,7 +243,7 @@ PredictedPath convertToPredictedPath( // first point const auto pt = lerpByLength(geometry_points, vehicle_pose_frenet.length); - geometry_msgs::PoseWithCovarianceStamped predicted_pose; + geometry_msgs::msg::PoseWithCovarianceStamped predicted_pose; predicted_pose.header.stamp = start_time; predicted_pose.pose.pose.position = pt; predicted_path.path.push_back(predicted_pose); @@ -248,7 +260,7 @@ PredictedPath convertToPredictedPath( length += travel_distance; const auto pt = lerpByLength(geometry_points, vehicle_pose_frenet.length + length); - geometry_msgs::PoseWithCovarianceStamped predicted_pose; + geometry_msgs::msg::PoseWithCovarianceStamped predicted_pose; predicted_pose.header.stamp = safeAddition(start_time, t); predicted_pose.pose.pose.position = pt; predicted_path.path.push_back(predicted_pose); @@ -258,23 +270,23 @@ PredictedPath convertToPredictedPath( } PredictedPath resamplePredictedPath( - const PredictedPath & input_path, const double resolution, const double duration) + const PredictedPath & input_path, const double resolution, const double duration, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) { PredictedPath resampled_path; - ros::Duration t_delta(resolution); - ros::Duration prediction_duration(duration); + rclcpp::Duration t_delta = rclcpp::Duration::from_seconds(resolution); + rclcpp::Duration prediction_duration = rclcpp::Duration::from_seconds(duration); - double min_distance = std::numeric_limits::max(); - ros::Time start_time = ros::Time::now(); - ros::Time end_time = ros::Time::now() + prediction_duration; + rclcpp::Time start_time = clock->now(); + rclcpp::Time end_time = clock->now() + prediction_duration; for (auto t = start_time; t < end_time; t += t_delta) { - geometry_msgs::Pose pose; - if (!lerpByTimeStamp(input_path, t, &pose)) { + geometry_msgs::msg::Pose pose; + if (!lerpByTimeStamp(input_path, t, &pose, logger, clock)) { continue; } - geometry_msgs::PoseWithCovarianceStamped predicted_pose; + geometry_msgs::msg::PoseWithCovarianceStamped predicted_pose; predicted_pose.header.frame_id = "map"; predicted_pose.header.stamp = t; predicted_pose.pose.pose = pose; @@ -284,8 +296,8 @@ PredictedPath resamplePredictedPath( return resampled_path; } -geometry_msgs::Pose lerpByPose( - const geometry_msgs::Pose & p1, const geometry_msgs::Pose & p2, const double t) +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) { tf2::Transform tf_transform1, tf_transform2; tf2::fromMsg(p1, tf_transform1); @@ -294,14 +306,16 @@ geometry_msgs::Pose lerpByPose( const auto & tf_quaternion = tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); - geometry_msgs::Pose pose; - pose.position = tf2::toMsg(tf_point, pose.position); + geometry_msgs::msg::Pose pose; + pose.position.x = tf_point.getX(); + pose.position.y = tf_point.getY(); + pose.position.z = tf_point.getZ(); pose.orientation = tf2::toMsg(tf_quaternion); return pose; } -geometry_msgs::Point lerpByPoint( - const geometry_msgs::Point & p1, const geometry_msgs::Point & p2, const double t) +geometry_msgs::msg::Point lerpByPoint( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double t) { tf2::Vector3 v1, v2; v1.setValue(p1.x, p1.y, p1.z); @@ -309,21 +323,21 @@ geometry_msgs::Point lerpByPoint( const auto lerped_point = v1.lerp(v2, t); - geometry_msgs::Point point; + geometry_msgs::msg::Point point; point.x = lerped_point.x(); point.y = lerped_point.y(); point.z = lerped_point.z(); return point; } -geometry_msgs::Point lerpByLength( - const std::vector & point_array, const double length) +geometry_msgs::msg::Point lerpByLength( + const std::vector & point_array, const double length) { - geometry_msgs::Point lerped_point; + geometry_msgs::msg::Point lerped_point; if (point_array.empty()) { return lerped_point; } - geometry_msgs::Point prev_pt = point_array.front(); + geometry_msgs::msg::Point prev_pt = point_array.front(); double accumulated_length = 0; for (const auto & pt : point_array) { const double distance = getDistance3d(prev_pt, pt); @@ -338,34 +352,36 @@ geometry_msgs::Point lerpByLength( } bool lerpByTimeStamp( - const PredictedPath & path, const ros::Time & t, geometry_msgs::Pose * lerped_pt) + const PredictedPath & path, const rclcpp::Time & t, geometry_msgs::msg::Pose * lerped_pt, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock) { if (lerped_pt == nullptr) { - ROS_WARN_STREAM_THROTTLE(1, "failed to lerp by time due to nullptr pt"); + RCLCPP_WARN_STREAM_THROTTLE(logger, *clock, 1.0, "failed to lerp by time due to nullptr pt"); return false; } if (path.path.empty()) { - ROS_WARN_STREAM_THROTTLE(1, "Empty path. Failed to interpolate path by time!"); + RCLCPP_WARN_STREAM_THROTTLE( + logger, *clock, 1.0, "Empty path. Failed to interpolate path by time!"); return false; } if (t < path.path.front().header.stamp) { - ROS_DEBUG_STREAM( - "failed to interpolate path by time!" - << std::endl - << "path start time: " << path.path.front().header.stamp << std::endl - << "path end time : " << path.path.back().header.stamp << std::endl - << "query time : " << t); + RCLCPP_DEBUG_STREAM( + logger, "failed to interpolate path by time!" + << std::endl + << "path start time: " << rclcpp::Time(path.path.front().header.stamp).seconds() << std::endl + << "path end time : " << rclcpp::Time(path.path.back().header.stamp).seconds() << std::endl + << "query time : " << t.seconds()); *lerped_pt = path.path.front().pose.pose; return false; } if (t > path.path.back().header.stamp) { - ROS_DEBUG_STREAM( - "failed to interpolate path by time!" - << std::endl - << "path start time: " << path.path.front().header.stamp << std::endl - << "path end time : " << path.path.back().header.stamp << std::endl - << "query time : " << t); + RCLCPP_DEBUG_STREAM( + logger, "failed to interpolate path by time!" + << std::endl + << "path start time: " << rclcpp::Time(path.path.front().header.stamp).seconds() << std::endl + << "path end time : " << rclcpp::Time(path.path.back().header.stamp).seconds() << std::endl + << "query time : " << t.seconds()); *lerped_pt = path.path.back().pose.pose; return false; @@ -375,38 +391,39 @@ bool lerpByTimeStamp( const auto & pt = path.path.at(i); const auto & prev_pt = path.path.at(i - 1); if (t <= pt.header.stamp) { - const ros::Duration duration = safeSubtraction(pt.header.stamp, prev_pt.header.stamp); + const rclcpp::Duration duration = safeSubtraction(pt.header.stamp, prev_pt.header.stamp); const auto offset = t - prev_pt.header.stamp; - const auto ratio = offset.toSec() / duration.toSec(); + const auto ratio = offset.seconds() / duration.seconds(); *lerped_pt = lerpByPose(prev_pt.pose.pose, pt.pose.pose, ratio); return true; } } - ROS_ERROR_STREAM("Something failed in function: " << __func__); + RCLCPP_ERROR_STREAM(logger, "Something failed in function: " << __func__); return false; } -double getDistance3d(const geometry_msgs::Point & p1, const geometry_msgs::Point & p2) +double getDistance3d(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2) + std::pow(p1.z - p2.z, 2)); } double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, - const double end_time, const double resolution) + const double end_time, const double resolution, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr & clock) { - ros::Duration t_delta(resolution); + rclcpp::Duration t_delta(resolution); double min_distance = std::numeric_limits::max(); - ros::Time ros_start_time = ros::Time::now() + ros::Duration(start_time); - ros::Time ros_end_time = ros::Time::now() + ros::Duration(end_time); + rclcpp::Time ros_start_time = clock->now() + rclcpp::Duration::from_seconds(start_time); + rclcpp::Time ros_end_time = clock->now() + rclcpp::Duration::from_seconds(end_time); const auto ego_path_point_array = convertToGeometryPointArray(ego_path); for (auto t = ros_start_time; t < ros_end_time; t += t_delta) { - geometry_msgs::Pose object_pose, ego_pose; - if (!lerpByTimeStamp(object_path, t, &object_pose)) { + geometry_msgs::msg::Pose object_pose, ego_pose; + if (!lerpByTimeStamp(object_path, t, &object_pose, logger, clock)) { continue; } - if (!lerpByTimeStamp(ego_path, t, &ego_pose)) { + if (!lerpByTimeStamp(ego_path, t, &ego_pose, logger, clock)) { continue; } double distance = getDistance3d(object_pose.position, ego_pose.position); @@ -419,9 +436,9 @@ double getDistanceBetweenPredictedPaths( // only works with consecutive lanes std::vector filterObjectsByLanelets( - const autoware_perception_msgs::DynamicObjectArray & objects, + const autoware_perception_msgs::msg::DynamicObjectArray & objects, const lanelet::ConstLanelets & target_lanelets, const double start_arc_length, - const double end_arc_length) + const double end_arc_length, const rclcpp::Logger & logger) { std::vector indices; if (target_lanelets.empty()) { @@ -439,7 +456,7 @@ std::vector filterObjectsByLanelets( const auto obj = objects.objects.at(i); // create object polygon Polygon obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { + if (!calcObjectPolygon(obj, &obj_polygon, logger)) { continue; } // create lanelet polygon @@ -461,8 +478,8 @@ std::vector filterObjectsByLanelets( // works with random lanelets std::vector filterObjectsByLanelets( - const autoware_perception_msgs::DynamicObjectArray & objects, - const lanelet::ConstLanelets & target_lanelets) + const autoware_perception_msgs::msg::DynamicObjectArray & objects, + const lanelet::ConstLanelets & target_lanelets, const rclcpp::Logger & logger) { std::vector indices; if (target_lanelets.empty()) { @@ -474,7 +491,7 @@ std::vector filterObjectsByLanelets( const auto obj = objects.objects.at(i); // create object polygon Polygon obj_polygon; - if (!calcObjectPolygon(obj, &obj_polygon)) { + if (!calcObjectPolygon(obj, &obj_polygon, logger)) { continue; } @@ -500,11 +517,12 @@ std::vector filterObjectsByLanelets( } bool calcObjectPolygon( - const autoware_perception_msgs::DynamicObject & object, Polygon * object_polygon) + const autoware_perception_msgs::msg::DynamicObject & object, Polygon * object_polygon, + const rclcpp::Logger & logger) { const double obj_x = object.state.pose_covariance.pose.position.x; const double obj_y = object.state.pose_covariance.pose.position.y; - if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double len_x = object.shape.dimensions.x; const double len_y = object.shape.dimensions.y; @@ -547,20 +565,20 @@ bool calcObjectPolygon( object_polygon->outer().push_back(Point(p3_obj.x(), p3_obj.y())); object_polygon->outer().push_back(Point(p4_obj.x(), p4_obj.y())); - } else if (object.shape.type == autoware_perception_msgs::Shape::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { const size_t N = 20; const double r = object.shape.dimensions.x / 2; for (size_t i = 0; i < N; ++i) { object_polygon->outer().push_back( Point(obj_x + r * std::cos(2.0 * M_PI / N * i), obj_y + r * std::sin(2.0 * M_PI / N * i))); } - } else if (object.shape.type == autoware_perception_msgs::Shape::POLYGON) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const auto obj_points = object.shape.footprint.points; for (const auto & obj_point : obj_points) { object_polygon->outer().push_back(Point(obj_point.x, obj_point.y)); } } else { - ROS_WARN("Object shape unknown!"); + RCLCPP_WARN(logger, "Object shape unknown!"); return false; } object_polygon->outer().push_back(object_polygon->outer().front()); @@ -569,15 +587,16 @@ bool calcObjectPolygon( } std::vector filterObjectsByPath( - const autoware_perception_msgs::DynamicObjectArray & objects, + const autoware_perception_msgs::msg::DynamicObjectArray & objects, const std::vector & object_indices, - const autoware_planning_msgs::PathWithLaneId & ego_path, const double vehicle_width) + const autoware_planning_msgs::msg::PathWithLaneId & ego_path, const double vehicle_width, + const rclcpp::Logger & logger) { std::vector indices; const auto ego_path_point_array = convertToGeometryPointArray(ego_path); for (const auto & i : object_indices) { Polygon obj_polygon; - if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon)) { + if (!calcObjectPolygon(objects.objects.at(i), &obj_polygon, logger)) { continue; } LineString ego_path_line; @@ -621,7 +640,8 @@ bool exists(std::vector vec, T element) bool setGoal( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, - const geometry_msgs::Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr) + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, PathWithLaneId * output_ptr, + const rclcpp::Logger & logger) { try { if (input.points.empty()) { @@ -635,7 +655,6 @@ bool setGoal( for (size_t i = 0; i < input.points.size(); ++i) { const double x = input.points.at(i).point.pose.position.x - goal.position.x; const double y = input.points.at(i).point.pose.position.y - goal.position.y; - const double z = input.points.at(i).point.pose.position.z - goal.position.z; const double dist = sqrt(x * x + y * y); if ( dist < search_radius_range && dist < min_dist && @@ -652,10 +671,9 @@ bool setGoal( size_t min_dist_out_of_range_index; { - for (size_t i = min_dist_index; 0 <= i; --i) { + for (size_t i = min_dist_index; ; --i) { const double x = input.points.at(i).point.pose.position.x - goal.position.x; const double y = input.points.at(i).point.pose.position.y - goal.position.y; - const double z = input.points.at(i).point.pose.position.z - goal.position.z; goal_z = input.points.at(i).point.pose.position.z; const double dist = sqrt(x * x + y * y); min_dist_out_of_range_index = i; @@ -667,13 +685,13 @@ bool setGoal( } } } - autoware_planning_msgs::PathPointWithLaneId refined_goal; + autoware_planning_msgs::msg::PathPointWithLaneId refined_goal; refined_goal.point.pose = goal; refined_goal.point.pose.position.z = goal_z; refined_goal.point.twist.linear.x = 0.0; refined_goal.lane_ids = input.points.back().lane_ids; - autoware_planning_msgs::PathPointWithLaneId pre_refined_goal; + autoware_planning_msgs::msg::PathPointWithLaneId pre_refined_goal; double roll, pitch, yaw; pre_refined_goal.point.pose = goal; tf2::Quaternion tf2_quaternion( @@ -696,13 +714,13 @@ bool setGoal( output_ptr->drivable_area = input.drivable_area; return true; } catch (std::out_of_range & ex) { - ROS_ERROR_STREAM("failed to set goal" << ex.what() << std::endl); + RCLCPP_ERROR_STREAM(logger, "failed to set goal" << ex.what() << std::endl); return false; } } -const geometry_msgs::Pose refineGoal( - const geometry_msgs::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) +const geometry_msgs::msg::Pose refineGoal( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet) { // return goal; const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position); @@ -718,7 +736,7 @@ const geometry_msgs::Pose refineGoal( return goal; } - geometry_msgs::Pose refined_goal; + geometry_msgs::msg::Pose refined_goal; { // find position const auto p1 = segment.front().basicPoint(); @@ -743,7 +761,7 @@ const geometry_msgs::Pose refineGoal( PathWithLaneId refinePath( const double search_radius_range, const double search_rad_range, const PathWithLaneId & input, - const geometry_msgs::Pose & goal, const int64_t goal_lane_id) + const geometry_msgs::msg::Pose & goal, const int64_t goal_lane_id, const rclcpp::Logger & logger) { PathWithLaneId filtered_path, path_with_goal; filtered_path = removeOverlappingPoints(input); @@ -754,8 +772,8 @@ PathWithLaneId refinePath( } if (setGoal( - search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, - &path_with_goal)) { + search_radius_range, search_rad_range, filtered_path, goal, goal_lane_id, &path_with_goal, + logger)) { return path_with_goal; } else { return filtered_path; @@ -773,8 +791,8 @@ bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal } // input lanes must be in sequence -nav_msgs::OccupancyGrid generateDrivableArea( - const lanelet::ConstLanelets & lanes, const geometry_msgs::PoseStamped & current_pose, +nav_msgs::msg::OccupancyGrid generateDrivableArea( + const lanelet::ConstLanelets & lanes, const geometry_msgs::msg::PoseStamped & current_pose, const double width, const double height, const double resolution, const double vehicle_length, const RouteHandler & route_handler) { @@ -785,8 +803,8 @@ nav_msgs::OccupancyGrid generateDrivableArea( drivable_lanes.insert(drivable_lanes.end(), lanes_after_goal.begin(), lanes_after_goal.end()); } - nav_msgs::OccupancyGrid occupancy_grid; - geometry_msgs::PoseStamped grid_origin; + nav_msgs::msg::OccupancyGrid occupancy_grid; + geometry_msgs::msg::PoseStamped grid_origin; // calculate grid origin { @@ -824,7 +842,7 @@ nav_msgs::OccupancyGrid generateDrivableArea( constexpr uint8_t occupied_space = 100; // get transform tf2::Stamped tf_grid2map, tf_map2grid; - tf2::fromMsg(grid_origin, tf_grid2map); + tf2::fromMsg(toTransformStamped(grid_origin), tf_grid2map); tf_map2grid.setData(tf_grid2map.inverse()); const auto geom_tf_map2grid = tf2::toMsg(tf_map2grid); @@ -855,10 +873,12 @@ nav_msgs::OccupancyGrid generateDrivableArea( occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, cv::Scalar(occupied_space)); std::vector cv_polygon; for (const auto & llt_pt : lane.polygon3d()) { - geometry_msgs::Point geom_pt = lanelet::utils::conversion::toGeomMsgPt(llt_pt); - geometry_msgs::Point transformed_geom_pt; - tf2::doTransform(geom_pt, transformed_geom_pt, geom_tf_map2grid); - cv_polygon.push_back(toCVPoint(transformed_geom_pt, width, height, resolution)); + geometry_msgs::msg::Point geom_pt = lanelet::utils::conversion::toGeomMsgPt(llt_pt); + geometry_msgs::msg::PointStamped geom_pt_stamped; + geom_pt_stamped.point = geom_pt; + geometry_msgs::msg::PointStamped transformed_geom_pt; + tf2::doTransform(geom_pt_stamped, transformed_geom_pt, geom_tf_map2grid); + cv_polygon.push_back(toCVPoint(transformed_geom_pt.point, width, height, resolution)); } cv_polygons.push_back(cv_polygon); cv_polygon_sizes.push_back(cv_polygon.size()); @@ -876,7 +896,7 @@ nav_msgs::OccupancyGrid generateDrivableArea( } double getDistanceToEndOfLane( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets) + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets) { const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); const double lanelet_length = lanelet::utils::getLaneletLength3d(lanelets); @@ -884,7 +904,7 @@ double getDistanceToEndOfLane( } double getDistanceToNextIntersection( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets) + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets) { const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); @@ -909,7 +929,7 @@ double getDistanceToNextIntersection( } double getDistanceToCrosswalk( - const geometry_msgs::Pose & current_pose, const lanelet::ConstLanelets & lanelets, + const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphContainer & overall_graphs) { const auto & arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose); @@ -950,7 +970,7 @@ double getDistanceToCrosswalk( for (const auto & point : points_intersection) { lanelet::ConstLanelets lanelets = {llt}; - geometry_msgs::Pose pose_point; + geometry_msgs::msg::Pose pose_point; pose_point.position.x = point.x(); pose_point.position.y = point.y(); const lanelet::ArcCoordinates & arc_crosswalk = @@ -974,7 +994,7 @@ double getDistanceToCrosswalk( } double getSignedDistance( - const geometry_msgs::Pose & current_pose, const geometry_msgs::Pose & goal_pose, + const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose, const lanelet::ConstLanelets & lanelets) { const auto arc_current = lanelet::utils::getArcCoordinates(lanelets, current_pose); @@ -992,10 +1012,10 @@ std::vector getIds(const lanelet::ConstLanelets & lanelets) return ids; } -autoware_planning_msgs::Path convertToPathFromPathWithLaneId( - const autoware_planning_msgs::PathWithLaneId & path_with_lane_id) +autoware_planning_msgs::msg::Path convertToPathFromPathWithLaneId( + const autoware_planning_msgs::msg::PathWithLaneId & path_with_lane_id) { - autoware_planning_msgs::Path path; + autoware_planning_msgs::msg::Path path; path.header = path_with_lane_id.header; path.drivable_area = path_with_lane_id.drivable_area; for (const auto & pt_with_lane_id : path_with_lane_id.points) { @@ -1005,7 +1025,7 @@ autoware_planning_msgs::Path convertToPathFromPathWithLaneId( } lanelet::Polygon3d getVehiclePolygon( - const geometry_msgs::Pose & vehicle_pose, const double vehicle_width, + const geometry_msgs::msg::Pose & vehicle_pose, const double vehicle_width, const double base_link2front) { tf2::Vector3 front_left, front_right, rear_left, rear_right; @@ -1033,17 +1053,17 @@ lanelet::Polygon3d getVehiclePolygon( return llt_poly; } -autoware_planning_msgs::PathPointWithLaneId insertStopPoint( - double length, autoware_planning_msgs::PathWithLaneId * path) +autoware_planning_msgs::msg::PathPointWithLaneId insertStopPoint( + double length, autoware_planning_msgs::msg::PathWithLaneId * path) { if (path->points.empty()) { - return autoware_planning_msgs::PathPointWithLaneId(); + return autoware_planning_msgs::msg::PathPointWithLaneId(); } double accumulated_length = 0; double insert_idx = 0; - geometry_msgs::Pose stop_pose; - for (int i = 1; i < path->points.size(); i++) { + geometry_msgs::msg::Pose stop_pose; + for (size_t i = 1; i < path->points.size(); i++) { const auto prev_pose = path->points.at(i - 1).point.pose; const auto curr_pose = path->points.at(i).point.pose; const double segment_length = getDistance3d(prev_pose.position, curr_pose.position); @@ -1056,13 +1076,13 @@ autoware_planning_msgs::PathPointWithLaneId insertStopPoint( } } - autoware_planning_msgs::PathPointWithLaneId stop_point; + autoware_planning_msgs::msg::PathPointWithLaneId stop_point; stop_point.lane_ids = path->points.at(insert_idx).lane_ids; stop_point.point.pose = stop_pose; stop_point.point.type = path->points.at(insert_idx).point.type; path->points.insert(path->points.begin() + insert_idx, stop_point); - for (int i = insert_idx; i < path->points.size(); i++) { - geometry_msgs::Twist zero_velocity; + for (size_t i = insert_idx; i < path->points.size(); i++) { + geometry_msgs::msg::Twist zero_velocity; path->points.at(insert_idx).point.twist = zero_velocity; } return stop_point; @@ -1071,7 +1091,7 @@ autoware_planning_msgs::PathPointWithLaneId insertStopPoint( /* * spline interpolation */ -SplineInterpolate::SplineInterpolate() {} +SplineInterpolate::SplineInterpolate(const rclcpp::Logger & logger) : logger_(logger) {} void SplineInterpolate::generateSpline( const std::vector & base_index, const std::vector & base_value) @@ -1086,7 +1106,7 @@ void SplineInterpolate::generateSpline( a_ = base_value; - for (size_t i = 0; i < N - 1; ++i) { + for (int i = 0; i < N - 1; ++i) { h_.push_back(base_index[i + 1] - base_index[i]); } @@ -1101,7 +1121,7 @@ void SplineInterpolate::generateSpline( b_.push_back(0.0); initialized_ = true; -}; +} double SplineInterpolate::getValue( const double & query, const std::vector & base_index) const @@ -1143,7 +1163,7 @@ bool SplineInterpolate::isIncrease(const std::vector & x) const if (x[i] > x[i + 1]) return false; } return true; -}; +} bool SplineInterpolate::isValidInput( const std::vector & base_index, const std::vector & base_value, @@ -1201,7 +1221,7 @@ std::vector SplineInterpolate::solveLinearSystem( ++num_iter; } - if (num_iter > max_iter) ROS_WARN("[interpolate] unconverged!"); + if (num_iter > max_iter) RCLCPP_WARN(logger_, "[interpolate] unconverged!"); return ans_next; }