Skip to content

Commit d75ea6e

Browse files
takayuki5168takayuki5168
authored
refactor(mission_planner): node refactoring (autowarefoundation#1940)
* refactor(mission_planner): node refactoring Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * organize files and directories Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * refactor Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
1 parent 767e765 commit d75ea6e

10 files changed

+228
-222
lines changed

‎planning/mission_planner/CMakeLists.txt

+4-4
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@ find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME}_component SHARED
8-
src/mission_planner/mission_planner.cpp
9-
src/mission_planner_lanelet2/mission_planner_lanelet2.cpp
10-
src/mission_planner_lanelet2/utility_functions.cpp
8+
src/mission_planner_interface.cpp
9+
src/mission_planner_lanelet2.cpp
10+
src/utility_functions.cpp
1111
)
1212

1313
rclcpp_components_register_node(${PROJECT_NAME}_component
@@ -16,7 +16,7 @@ rclcpp_components_register_node(${PROJECT_NAME}_component
1616
)
1717

1818
ament_auto_add_library(goal_pose_visualizer_component SHARED
19-
src/goal_pose_visualizer/goal_pose_visualizer.cpp
19+
src/goal_pose_visualizer.cpp
2020
)
2121

2222
rclcpp_components_register_node(goal_pose_visualizer_component

‎planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp ‎planning/mission_planner/include/mission_planner/goal_pose_visualizer.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_
16-
#define GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_
15+
#ifndef MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_
16+
#define MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_
1717

1818
#include <rclcpp/rclcpp.hpp>
1919

@@ -36,4 +36,4 @@ class GoalPoseVisualizer : public rclcpp::Node
3636
};
3737

3838
} // namespace mission_planner
39-
#endif // GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_
39+
#endif // MISSION_PLANNER__GOAL_POSE_VISUALIZER_HPP_

‎planning/mission_planner/src/mission_planner/mission_planner.hpp ‎planning/mission_planner/include/mission_planner/mission_planner_interface.hpp

+19-17
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,10 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef MISSION_PLANNER__MISSION_PLANNER_HPP_
16-
#define MISSION_PLANNER__MISSION_PLANNER_HPP_
15+
#ifndef MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_
16+
#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_
17+
18+
#include <boost/optional.hpp>
1719

1820
#include <string>
1921
#include <vector>
@@ -30,45 +32,45 @@
3032
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
3133
#include <geometry_msgs/msg/pose_stamped.hpp>
3234
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
35+
#include <nav_msgs/msg/odometry.hpp>
3336

3437
namespace mission_planner
3538
{
36-
class MissionPlanner : public rclcpp::Node
39+
class MissionPlannerInterface : public rclcpp::Node
3740
{
3841
protected:
39-
MissionPlanner(const std::string & node_name, const rclcpp::NodeOptions & node_options);
42+
MissionPlannerInterface(const std::string & node_name, const rclcpp::NodeOptions & node_options);
4043

41-
geometry_msgs::msg::PoseStamped goal_pose_;
42-
geometry_msgs::msg::PoseStamped start_pose_;
43-
std::vector<geometry_msgs::msg::PoseStamped> checkpoints_;
44+
geometry_msgs::msg::Pose::ConstSharedPtr ego_pose_;
45+
std::vector<geometry_msgs::msg::Pose> check_points_;
4446

45-
std::string base_link_frame_;
4647
std::string map_frame_;
4748

4849
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
4950

5051
virtual bool is_routing_graph_ready() const = 0;
51-
virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route() = 0;
52+
virtual autoware_auto_planning_msgs::msg::HADMapRoute plan_route(
53+
const std::vector<geometry_msgs::msg::Pose> & check_points) = 0;
5254
virtual void visualize_route(
5355
const autoware_auto_planning_msgs::msg::HADMapRoute & route) const = 0;
5456
virtual void publish_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const;
5557

5658
private:
5759
rclcpp::Publisher<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr route_publisher_;
5860
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_subscriber_;
59-
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr checkpoint_subscriber_;
61+
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr check_point_subscriber_;
62+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_subscriber_;
6063

6164
tf2_ros::Buffer tf_buffer_;
6265
tf2_ros::TransformListener tf_listener_;
6366

64-
bool get_ego_vehicle_pose(geometry_msgs::msg::PoseStamped * ego_vehicle_pose);
67+
void odometry_callback(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
6568
void goal_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr goal_msg_ptr);
66-
void checkpoint_callback(
67-
const geometry_msgs::msg::PoseStamped::ConstSharedPtr checkpoint_msg_ptr);
68-
bool transform_pose(
69-
const geometry_msgs::msg::PoseStamped & input_pose,
70-
geometry_msgs::msg::PoseStamped * output_pose, const std::string & target_frame);
69+
void check_point_callback(
70+
const geometry_msgs::msg::PoseStamped::ConstSharedPtr check_point_msg_ptr);
71+
boost::optional<geometry_msgs::msg::PoseStamped> transform_pose(
72+
const geometry_msgs::msg::PoseStamped & input_pose, const std::string & target_frame);
7173
};
7274

7375
} // namespace mission_planner
74-
#endif // MISSION_PLANNER__MISSION_PLANNER_HPP_
76+
#endif // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_

‎planning/mission_planner/src/mission_planner_lanelet2/mission_planner_lanelet2.hpp ‎planning/mission_planner/include/mission_planner/mission_planner_lanelet2.hpp

+10-8
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_
16-
#define MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_
15+
#ifndef MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_
16+
#define MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_
1717

1818
#include <string>
1919
#include <vector>
@@ -25,7 +25,7 @@
2525
#include <tf2_ros/transform_listener.h>
2626

2727
// Autoware
28-
#include "../mission_planner/mission_planner.hpp"
28+
#include "mission_planner/mission_planner_interface.hpp"
2929

3030
#include <route_handler/route_handler.hpp>
3131

@@ -40,7 +40,7 @@
4040
namespace mission_planner
4141
{
4242
using RouteSections = std::vector<autoware_auto_mapping_msgs::msg::HADMapSegment>;
43-
class MissionPlannerLanelet2 : public MissionPlanner
43+
class MissionPlannerLanelet2 : public MissionPlannerInterface
4444
{
4545
public:
4646
explicit MissionPlannerLanelet2(const rclcpp::NodeOptions & node_options);
@@ -57,14 +57,16 @@ class MissionPlannerLanelet2 : public MissionPlanner
5757
rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_subscriber_;
5858

5959
void map_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
60-
bool is_goal_valid() const;
61-
void refine_goal_height(const RouteSections & route_sections);
60+
bool is_goal_valid(const geometry_msgs::msg::Pose & goal_pose) const;
61+
geometry_msgs::msg::Pose refine_goal_height(
62+
const RouteSections & route_sections, const geometry_msgs::msg::Pose & goal_pose) const;
6263

6364
// virtual functions
6465
bool is_routing_graph_ready() const override;
65-
autoware_auto_planning_msgs::msg::HADMapRoute plan_route() override;
66+
autoware_auto_planning_msgs::msg::HADMapRoute plan_route(
67+
const std::vector<geometry_msgs::msg::Pose> & check_points) override;
6668
void visualize_route(const autoware_auto_planning_msgs::msg::HADMapRoute & route) const override;
6769
};
6870
} // namespace mission_planner
6971

70-
#endif // MISSION_PLANNER_LANELET2__MISSION_PLANNER_LANELET2_HPP_
72+
#endif // MISSION_PLANNER__MISSION_PLANNER_LANELET2_HPP_

‎planning/mission_planner/src/mission_planner_lanelet2/utility_functions.hpp ‎planning/mission_planner/include/mission_planner/utility_functions.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_
16-
#define MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_
15+
#ifndef MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_
16+
#define MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_
1717
#include <rclcpp/rclcpp.hpp>
1818

1919
#include <geometry_msgs/msg/pose.hpp>
@@ -43,4 +43,4 @@ void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, doub
4343
void insert_marker_array(
4444
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
4545
std::string to_string(const geometry_msgs::msg::Pose & pose);
46-
#endif // MISSION_PLANNER_LANELET2__UTILITY_FUNCTIONS_HPP_
46+
#endif // MISSION_PLANNER__UTILITY_FUNCTIONS_HPP_

‎planning/mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp ‎planning/mission_planner/src/goal_pose_visualizer.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "goal_pose_visualizer.hpp"
15+
#include "mission_planner/goal_pose_visualizer.hpp"
1616

1717
namespace mission_planner
1818
{

‎planning/mission_planner/src/mission_planner/mission_planner.cpp

-152
This file was deleted.

0 commit comments

Comments
 (0)