Merge 5f54386d on remote branch

Change-Id: Iecb623673d8bf977db5f0c02a991f6c2dac34227

inventory_scan/inventory_scan/include/data_def.hpp

@@ -36,6 +36,7 @@ public:
 
   PathManager() = default;
   bool add_station(uint32_t id, const Point2D& pos);
+  void get_station(uint32_t id, Point2D& pos);
   bool delete_station(uint32_t id);
   void get_stations(std::unordered_map<uint32_t, Point2D>& station);
   bool add_path(uint32_t id1, uint32_t id2);
@@ -57,8 +58,8 @@ class NaviState
 public:
   const static uint32_t IDLE = 0;
   const static uint32_t NAVIGATING = 1;
-  const static uint32_t NAVI_PHASE_DONE = 2;
-  const static uint32_t GOING_HOME = 3;
+  const static uint32_t GOING_HOME = 2;
+  const static uint32_t NAVI_ERR = 3;
 
   NaviState();
   uint32_t get_state(void);

inventory_scan/inventory_scan/include/nodes_def.hpp

@@ -33,6 +33,7 @@ class PathManagerNode : public rclcpp::Node
 public:
   using PathService = inventory_scan_msgs::srv::ScanPath;
   using VirtualPath = qrb_ros_navigation_msgs::srv::VirtualPath;
+  using VirtualPathFuture = rclcpp::Client<VirtualPath>::SharedFuture;
   explicit PathManagerNode(std::shared_ptr<PathManager> path,
     std::shared_ptr<NaviState> navi_state);
   ~PathManagerNode() = default;
@@ -45,17 +46,26 @@ private:
   rclcpp::Client<VirtualPath>::SharedPtr vp_client_{ nullptr };
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  std::mutex lock_;
+  std::condition_variable cv_;
+  uint8_t vp_response_ret_;
 
   void service_callback(const std::shared_ptr<rmw_request_id_t> request_header,
       const std::shared_ptr<PathService::Request> request,
       std::shared_ptr<PathService::Response> response);
 
-  uint32_t virtual_path_add_station(const Point2D & pos);
-  uint32_t virtual_path_delete_station(uint32_t id);
-  bool query_pose(Point2D & pos);
-  void virtual_path_add_path(uint32_t id1, uint32_t id2);
-  void virtual_path_delete_path(uint32_t id1, uint32_t id2);
-  void virtual_path_add_full_path(const std::vector<uint32_t> & path);
+  void init_station_list(std::vector<uint32_t>& list, size_t& len);
+  void init_station(const std::vector<uint32_t>& list, size_t len);
+  void init_path(const std::vector<uint32_t>& list, size_t len);
+  uint32_t virtual_path_add_station(const Point2D& pos);
+  bool virtual_path_delete_station(uint32_t id);
+  bool query_pose(Point2D& pos);
+  bool virtual_path_add_path(uint32_t id1, uint32_t id2);
+  bool virtual_path_delete_path(uint32_t id1, uint32_t id2);
+
+  void init();
 };
 
 class ScanNode : public rclcpp::Node
@@ -72,7 +82,7 @@ private:
   void goal_response_callback(GoalHandleVision::SharedPtr handle);
   void feedback_callback(GoalHandleVision::SharedPtr handle,
       const std::shared_ptr<const VisionAction::Feedback> feedback);
-  void result_callback(const GoalHandleVision::WrappedResult & result);
+  void result_callback(const GoalHandleVision::WrappedResult& result);
 
   void execute(void);
   std::shared_ptr<SyncState> sync_state_;
@@ -83,6 +93,11 @@ private:
 class NaviNode : public rclcpp::Node
 {
 public:
+  const static uint32_t NAVI_IDLE = 0;
+  const static uint32_t NAVI_START = 1;
+  const static uint32_t NAVI_FINISH = 2;
+  const static uint32_t NAVI_ERROR = 3;
+
   using NaviService = inventory_scan_msgs::srv::ScanControl;
 
   // navigation includes p2p and followpth
@@ -103,6 +118,7 @@ private:
       std::shared_ptr<NaviService::Response> response);
 
   void execute(const std::vector<uint32_t>& full_path);
+  void go_home();
   // for Navigation Action client
   rclcpp_action::Client<NavigateToPose>::SharedPtr navi_client_{ nullptr };
   GoalHandleNavigate::SharedPtr goal_handle_;
@@ -110,21 +126,31 @@ private:
   void navi_feedback_callback(GoalHandleNavigate::SharedPtr,
       const std::shared_ptr<const NavigateToPose::Feedback> feedback);
   void navi_result_callback(const GoalHandleNavigate::WrappedResult & result);
+  void init();
 
   // compute path service client
   rclcpp::Client<ComputeFollowPath>::SharedPtr fp_client_{ nullptr };
   rclcpp::Client<ComputeP2pPath>::SharedPtr p2p_client_{ nullptr };
 
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+
   std::shared_ptr<NaviState> navi_state_;
   std::shared_ptr<PathManager> path_;
   std::shared_ptr<SyncState> sync_state_;
+
+  uint32_t navi_result_;
+  bool navi_server_ready_;
+  std::mutex lock_;
+  std::condition_variable cv_;
 };
 
 class PubNode : public rclcpp::Node
 {
 public:
   explicit PubNode(std::shared_ptr<MapManager> map_manager,
-    std::shared_ptr<NaviState> navi_state, std::shared_ptr<GoodsManager> goods);
+                   std::shared_ptr<NaviState> navi_state,
+                   std::shared_ptr<GoodsManager> goods);
 
 private:
   rclcpp::Publisher<inventory_scan_msgs::msg::ScanResult>::SharedPtr result_pub_;

inventory_scan/inventory_scan/src/data_impl.cpp

@@ -17,12 +17,21 @@ bool PathManager::add_station(uint32_t id, const Point2D & pos)
   auto ret = station_.insert({ id, pos });
   lock_.unlock();
 
+  std::cout << "add station: " << id <<std::endl;
+
   if (!ret.second)
     return false;
   else
     return true;
 }
 
+void PathManager::get_station(uint32_t id, Point2D& pos)
+{
+  lock_.lock();
+  pos = station_.at(id);
+  lock_.unlock();
+}
+
 bool PathManager::delete_station(uint32_t id)
 {
   lock_.lock();
@@ -372,12 +381,13 @@ void MapManager::pose_to_pixel(const Point2D & pose,
     double ori_y,
     const cv::Mat & map)
 {
-  float x = (float)(pose.x - ori_x);
-  float y = (float)(pose.y - ori_y);
+  double x = pose.x - ori_x;
+  double y = pose.y - ori_y;
 
   // point to pixel in image
   pixel.x = static_cast<int>(x / res + 0.5);
-  pixel.y = map.rows - static_cast<int>(y / res + 0.5);
+  //pixel.y = map.rows - static_cast<int>(y / res + 0.5);
+  pixel.y = static_cast<int>(y / res + 0.5);
 }
 
 bool MapManager::is_pixel_in_image(const cv::Point & pixel, const cv::Mat & image)