Merge pull request #11 from LARR-Planning/thread_check

Add data type and basic functions

README.md

@@ -1 +1 @@
-# larr_planner
+# larr-planner

larr_planner/include/larr_planner/type_manager/TypeManager.h

@@ -0,0 +1,21 @@
+
+
+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};
+};
+
+} // namespace larr_planner

larr_planner/include/larr_planner/wrapper/Wrapper.h

@@ -5,4 +5,17 @@
 #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:
+};
+
+} // namespace larr_planner
+
 #endif // LARR_PLANNER_WRAPPER_H

larr_planner/src/type_manager/TypeManager.cc

@@ -0,0 +1 @@
+#include "larr_planner/type_manager/TypeManager.h"

larr_planner/src/wrapper/Wrapper.cc

@@ -1,4 +1,5 @@
 //
 // Created by larr on 24. 5. 17.
 //
-#include "larr_planner/wrapper/Wrapper.h"
+#include "larr_planner/wrapper/Wrapper.h"
+void larr_planner::Wrapper::SetRobotState(const larr_planner::RobotState &robot_state) {}

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 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
         $<INSTALL_INTERFACE:include>

larr_planner_ros2/include/larr_planner_ros2/planning_server/planning_server.h

@@ -7,13 +7,24 @@
 
 // #include <pcl/PCLPointCloud2.h>
 // #include <pcl_conversions/pcl_conversions.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <larr_planner/wrapper/Wrapper.h>
 #include <rclcpp/rclcpp.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+using RobotStateMsg = geometry_msgs::msg::PoseStamped;
+using StateSubscriber = rclcpp::Subscription<RobotStateMsg>::SharedPtr;
 
 namespace larr_planner {
 
 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);
 };

larr_planner_ros2/src/planning_server/planning_server.cc

@@ -4,5 +4,29 @@
 #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) {}
+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<RobotStateMsg>(
+      "~/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;
+}

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<larr_planner::PlanningServer>(options);
+
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(planning_server);
   executor.spin();
   rclcpp::shutdown();
+
   return 0;
 }