From 51f7d7bfec2b5169170fa43a136533e4f3c4c883 Mon Sep 17 00:00:00 2001 From: yunwoo Date: Fri, 17 May 2024 17:35:26 +0900 Subject: [PATCH 1/3] Add data type and basic functions --- README.md | 2 +- .../larr_planner/type_manager/TypeManager.h | 24 +++++++++++++++ .../include/larr_planner/wrapper/Wrapper.h | 14 +++++++++ larr_planner/src/type_manager/TypeManager.cc | 3 ++ larr_planner/src/wrapper/Wrapper.cc | 5 +++- larr_planner_ros2/CMakeLists.txt | 2 +- .../planning_server/planning_server.h | 16 +++++++++- .../src/planning_server/planning_server.cc | 30 +++++++++++++++++-- .../planning_server/planning_server_node.cc | 5 ++-- 9 files changed, 93 insertions(+), 8 deletions(-) create mode 100644 larr_planner/include/larr_planner/type_manager/TypeManager.h create mode 100644 larr_planner/src/type_manager/TypeManager.cc diff --git a/README.md b/README.md index 57b6b95..ad2a303 100644 --- a/README.md +++ b/README.md @@ -1 +1 @@ -# larr_planner +# larr-planner diff --git a/larr_planner/include/larr_planner/type_manager/TypeManager.h b/larr_planner/include/larr_planner/type_manager/TypeManager.h new file mode 100644 index 0000000..8710d40 --- /dev/null +++ b/larr_planner/include/larr_planner/type_manager/TypeManager.h @@ -0,0 +1,24 @@ + + +namespace larr_planner{ +struct Position{ + double px; + double py; + double pz; +}; +struct Quaternion{ + double qx; + double qy; + double qz; + double qw; +}; +struct RobotState { + double t_sec{0.0}; + Position pos{0.0,0.0,0.0}; + Quaternion orientation{0.0,0.0,0.0,1.0}; + +}; + + + +} diff --git a/larr_planner/include/larr_planner/wrapper/Wrapper.h b/larr_planner/include/larr_planner/wrapper/Wrapper.h index 0b3f0b3..b35b33c 100644 --- a/larr_planner/include/larr_planner/wrapper/Wrapper.h +++ b/larr_planner/include/larr_planner/wrapper/Wrapper.h @@ -5,4 +5,18 @@ #ifndef LARR_PLANNER_WRAPPER_H #define LARR_PLANNER_WRAPPER_H +#include "larr_planner/type_manager/TypeManager.h" + +namespace larr_planner{ + +class Wrapper{ +public: + void SetRobotState(const RobotState &robot_state); +private: + +}; + +} + + #endif // LARR_PLANNER_WRAPPER_H diff --git a/larr_planner/src/type_manager/TypeManager.cc b/larr_planner/src/type_manager/TypeManager.cc new file mode 100644 index 0000000..5e0d4d8 --- /dev/null +++ b/larr_planner/src/type_manager/TypeManager.cc @@ -0,0 +1,3 @@ +#include "larr_planner/type_manager/TypeManager.h" + + diff --git a/larr_planner/src/wrapper/Wrapper.cc b/larr_planner/src/wrapper/Wrapper.cc index af0918f..46947d3 100644 --- a/larr_planner/src/wrapper/Wrapper.cc +++ b/larr_planner/src/wrapper/Wrapper.cc @@ -1,4 +1,7 @@ // // Created by larr on 24. 5. 17. // -#include "larr_planner/wrapper/Wrapper.h" \ No newline at end of file +#include "larr_planner/wrapper/Wrapper.h" +void larr_planner::Wrapper::SetRobotState(const larr_planner::RobotState &robot_state) { + +} diff --git a/larr_planner_ros2/CMakeLists.txt b/larr_planner_ros2/CMakeLists.txt index 3587d29..da2ff37 100644 --- a/larr_planner_ros2/CMakeLists.txt +++ b/larr_planner_ros2/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(PCL REQUIRED) find_package(larr_planner REQUIRED) #find_package(los_keeper_msgs REQUIRED) add_library(planning_server src/planning_server/planning_server.cc) -ament_target_dependencies(planning_server rclcpp sensor_msgs geometry_msgs) +ament_target_dependencies(planning_server rclcpp sensor_msgs geometry_msgs visualization_msgs) target_include_directories( planning_server PUBLIC $ $ diff --git a/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h b/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h index 986a23d..f4dc00f 100644 --- a/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h +++ b/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h @@ -8,12 +8,26 @@ // #include // #include #include +#include +#include +#include + +using RobotStateMsg = geometry_msgs::msg::PoseStamped; +using StateSubscriber = rclcpp::Subscription::SharedPtr; + namespace larr_planner { -class PlanningServer : public rclcpp::Node { +class PlanningServer : public rclcpp::Node { private: + Wrapper *wrapper_ptr_; + StateSubscriber state_subscriber_; + + void RobotStateCallback(const RobotStateMsg::ConstSharedPtr &msg); + RobotState ConvertToRobotState(const RobotStateMsg &state_msg); + + public: PlanningServer(const rclcpp::NodeOptions &options_input); }; diff --git a/larr_planner_ros2/src/planning_server/planning_server.cc b/larr_planner_ros2/src/planning_server/planning_server.cc index fd2c9af..87df670 100644 --- a/larr_planner_ros2/src/planning_server/planning_server.cc +++ b/larr_planner_ros2/src/planning_server/planning_server.cc @@ -4,5 +4,31 @@ #include "larr_planner_ros2/planning_server/planning_server.h" using namespace larr_planner; -PlanningServer::PlanningServer(const rclcpp::NodeOptions &options_input) - : Node("planning_server_node", options_input) {} \ No newline at end of file +PlanningServer::PlanningServer(const rclcpp::NodeOptions &options_input) : Node("planning_server_node", options_input) { + rclcpp::SubscriptionOptions options; + // Subscribe Robot State {POSITION, ORIENTATION} + options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + state_subscriber_ = create_subscription( + "~/state", rclcpp::QoS(1), std::bind(&PlanningServer::RobotStateCallback,this, std::placeholders::_1),options); + + +} + +void PlanningServer::RobotStateCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg) { + wrapper_ptr_-> SetRobotState(ConvertToRobotState(*msg)); +} + +RobotState PlanningServer::ConvertToRobotState(const RobotStateMsg &state_msg) { + RobotState state; + state.t_sec = state_msg.header.stamp.sec + state_msg.header.stamp.nanosec*1E-9; + state.pos.px = state_msg.pose.position.x; + state.pos.py = state_msg.pose.position.y; + state.pos.pz = state_msg.pose.position.z; + + state.orientation.qw = state_msg.pose.orientation.w; + state.orientation.qx = state_msg.pose.orientation.x; + state.orientation.qy = state_msg.pose.orientation.y; + state.orientation.qz = state_msg.pose.orientation.z; + + return state; +} diff --git a/larr_planner_ros2/src/planning_server/planning_server_node.cc b/larr_planner_ros2/src/planning_server/planning_server_node.cc index 1212582..08a5847 100644 --- a/larr_planner_ros2/src/planning_server/planning_server_node.cc +++ b/larr_planner_ros2/src/planning_server/planning_server_node.cc @@ -1,18 +1,19 @@ // // Created by larr on 24. 5. 17. // -// #include "los_keeper_ros2/los_server/los_server.h" -// #include "larr_planner_ros2/planning_server/planning_server.h" #include "larr_planner_ros2/planning_server/planning_server.h" int main(int argc, char *argv[]) { rclcpp::init(argc, argv); + auto options = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true); auto planning_server = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(planning_server); executor.spin(); rclcpp::shutdown(); + return 0; } \ No newline at end of file From 2e5dec21c3a05ecf93a3ec003d80011a18917800 Mon Sep 17 00:00:00 2001 From: yunwoo Date: Sun, 19 May 2024 00:04:26 +0900 Subject: [PATCH 2/3] Add data type and basic functions --- larr_planner_ros2/src/planning_server/planning_server.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/larr_planner_ros2/src/planning_server/planning_server.cc b/larr_planner_ros2/src/planning_server/planning_server.cc index 87df670..dd109a0 100644 --- a/larr_planner_ros2/src/planning_server/planning_server.cc +++ b/larr_planner_ros2/src/planning_server/planning_server.cc @@ -10,8 +10,6 @@ PlanningServer::PlanningServer(const rclcpp::NodeOptions &options_input) : Node( options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); state_subscriber_ = create_subscription( "~/state", rclcpp::QoS(1), std::bind(&PlanningServer::RobotStateCallback,this, std::placeholders::_1),options); - - } void PlanningServer::RobotStateCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg) { From b565a75172c7cc6fcae3c6d1050bd6f8a4e014f7 Mon Sep 17 00:00:00 2001 From: yunwoo Date: Sun, 19 May 2024 00:06:36 +0900 Subject: [PATCH 3/3] Change format --- .../larr_planner/type_manager/TypeManager.h | 15 ++++++--------- .../include/larr_planner/wrapper/Wrapper.h | 9 ++++----- larr_planner/src/type_manager/TypeManager.cc | 2 -- larr_planner/src/wrapper/Wrapper.cc | 4 +--- .../planning_server/planning_server.h | 7 ++----- .../src/planning_server/planning_server.cc | 6 +++--- 6 files changed, 16 insertions(+), 27 deletions(-) diff --git a/larr_planner/include/larr_planner/type_manager/TypeManager.h b/larr_planner/include/larr_planner/type_manager/TypeManager.h index 8710d40..0f64bf6 100644 --- a/larr_planner/include/larr_planner/type_manager/TypeManager.h +++ b/larr_planner/include/larr_planner/type_manager/TypeManager.h @@ -1,12 +1,12 @@ -namespace larr_planner{ -struct Position{ +namespace larr_planner { +struct Position { double px; double py; double pz; }; -struct Quaternion{ +struct Quaternion { double qx; double qy; double qz; @@ -14,11 +14,8 @@ struct Quaternion{ }; struct RobotState { double t_sec{0.0}; - Position pos{0.0,0.0,0.0}; - Quaternion orientation{0.0,0.0,0.0,1.0}; - + Position pos{0.0, 0.0, 0.0}; + Quaternion orientation{0.0, 0.0, 0.0, 1.0}; }; - - -} +} // namespace larr_planner diff --git a/larr_planner/include/larr_planner/wrapper/Wrapper.h b/larr_planner/include/larr_planner/wrapper/Wrapper.h index b35b33c..a0f9ad9 100644 --- a/larr_planner/include/larr_planner/wrapper/Wrapper.h +++ b/larr_planner/include/larr_planner/wrapper/Wrapper.h @@ -7,16 +7,15 @@ #include "larr_planner/type_manager/TypeManager.h" -namespace larr_planner{ +namespace larr_planner { -class Wrapper{ +class Wrapper { public: void SetRobotState(const RobotState &robot_state); -private: +private: }; -} - +} // namespace larr_planner #endif // LARR_PLANNER_WRAPPER_H diff --git a/larr_planner/src/type_manager/TypeManager.cc b/larr_planner/src/type_manager/TypeManager.cc index 5e0d4d8..1442ada 100644 --- a/larr_planner/src/type_manager/TypeManager.cc +++ b/larr_planner/src/type_manager/TypeManager.cc @@ -1,3 +1 @@ #include "larr_planner/type_manager/TypeManager.h" - - diff --git a/larr_planner/src/wrapper/Wrapper.cc b/larr_planner/src/wrapper/Wrapper.cc index 46947d3..6d0f4e1 100644 --- a/larr_planner/src/wrapper/Wrapper.cc +++ b/larr_planner/src/wrapper/Wrapper.cc @@ -2,6 +2,4 @@ // Created by larr on 24. 5. 17. // #include "larr_planner/wrapper/Wrapper.h" -void larr_planner::Wrapper::SetRobotState(const larr_planner::RobotState &robot_state) { - -} +void larr_planner::Wrapper::SetRobotState(const larr_planner::RobotState &robot_state) {} diff --git a/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h b/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h index f4dc00f..992643f 100644 --- a/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h +++ b/larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h @@ -7,18 +7,16 @@ // #include // #include -#include -#include #include +#include +#include #include using RobotStateMsg = geometry_msgs::msg::PoseStamped; using StateSubscriber = rclcpp::Subscription::SharedPtr; - namespace larr_planner { - class PlanningServer : public rclcpp::Node { private: Wrapper *wrapper_ptr_; @@ -27,7 +25,6 @@ class PlanningServer : public rclcpp::Node { void RobotStateCallback(const RobotStateMsg::ConstSharedPtr &msg); RobotState ConvertToRobotState(const RobotStateMsg &state_msg); - public: PlanningServer(const rclcpp::NodeOptions &options_input); }; diff --git a/larr_planner_ros2/src/planning_server/planning_server.cc b/larr_planner_ros2/src/planning_server/planning_server.cc index dd109a0..3910db8 100644 --- a/larr_planner_ros2/src/planning_server/planning_server.cc +++ b/larr_planner_ros2/src/planning_server/planning_server.cc @@ -9,16 +9,16 @@ PlanningServer::PlanningServer(const rclcpp::NodeOptions &options_input) : Node( // Subscribe Robot State {POSITION, ORIENTATION} options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); state_subscriber_ = create_subscription( - "~/state", rclcpp::QoS(1), std::bind(&PlanningServer::RobotStateCallback,this, std::placeholders::_1),options); + "~/state", rclcpp::QoS(1), std::bind(&PlanningServer::RobotStateCallback, this, std::placeholders::_1), options); } void PlanningServer::RobotStateCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &msg) { - wrapper_ptr_-> SetRobotState(ConvertToRobotState(*msg)); + wrapper_ptr_->SetRobotState(ConvertToRobotState(*msg)); } RobotState PlanningServer::ConvertToRobotState(const RobotStateMsg &state_msg) { RobotState state; - state.t_sec = state_msg.header.stamp.sec + state_msg.header.stamp.nanosec*1E-9; + state.t_sec = state_msg.header.stamp.sec + state_msg.header.stamp.nanosec * 1E-9; state.pos.px = state_msg.pose.position.x; state.pos.py = state_msg.pose.position.y; state.pos.pz = state_msg.pose.position.z;