Several edits for multi-instance FSM + simulation + correct nlopt

eliabntt · Apr 29, 2022 · 5d61a8c · 5d61a8c
1 parent 759729e
commit 5d61a8c
Show file tree

Hide file tree

Showing 69 changed files with 3,349 additions and 2,050 deletions.
diff --git a/fuel_planner/active_perception/CMakeLists.txt b/fuel_planner/active_perception/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(active_perception)
 
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS "-std=c++14")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(PCL 1.7 REQUIRED)

diff --git a/fuel_planner/active_perception/src/heading_planner.cpp b/fuel_planner/active_perception/src/heading_planner.cpp
@@ -100,9 +100,9 @@ void Graph::dijkstraSearch(const int& start, const int& goal, vector<YawVertex::
 }
 
 HeadingPlanner::HeadingPlanner(ros::NodeHandle& nh) {
-  frontier_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/heading_planner/frontier", 20);
-  visib_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/heading_planner/visib", 20);
-  box_pub_ = nh.advertise<visualization_msgs::Marker>("/heading_planner/box", 20);
+  frontier_pub_ = nh.advertise<sensor_msgs::PointCloud2>("heading_planner/frontier", 20);
+  visib_pub_ = nh.advertise<sensor_msgs::PointCloud2>("heading_planner/visib", 20);
+  box_pub_ = nh.advertise<visualization_msgs::Marker>("heading_planner/box", 20);
 
   nh.param("heading_planner/yaw_diff", yaw_diff_, -1.0);
   nh.param("heading_planner/lambda1", lambda1_, -1.0);

diff --git a/fuel_planner/bspline/CMakeLists.txt b/fuel_planner/bspline/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(bspline)
 
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS "-std=c++14")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(Eigen3 REQUIRED)

diff --git a/fuel_planner/bspline_opt/CMakeLists.txt b/fuel_planner/bspline_opt/CMakeLists.txt
@@ -2,11 +2,12 @@ cmake_minimum_required(VERSION 2.8.3)
 project(bspline_opt)
 
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS "-std=c++14")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(Eigen3 REQUIRED)
 find_package(PCL 1.7 REQUIRED)
+find_package(nlopt)
 
 find_package(catkin REQUIRED COMPONENTS
   roscpp
@@ -16,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS
   plan_env
   active_perception
   cv_bridge
-  nlopt
 )
 
 catkin_package(
@@ -32,12 +32,13 @@ include_directories(
     ${catkin_INCLUDE_DIRS}
     ${Eigen3_INCLUDE_DIRS} 
     ${PCL_INCLUDE_DIRS}
+    ${nlopt_INCLUDE_DIRS}
 )
 
 add_library( bspline_opt 
     src/bspline_optimizer.cpp 
     )
 target_link_libraries( bspline_opt
     ${catkin_LIBRARIES} 
-    # /usr/local/lib/libnlopt.so
+    nlopt
     )  
diff --git a/fuel_planner/exploration_manager/CMakeLists.txt b/fuel_planner/exploration_manager/CMakeLists.txt
@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(exploration_manager)
 
 set(CMAKE_BUILD_TYPE "Release")
-set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS "-std=c++14")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
 
 find_package(Eigen3 REQUIRED)
@@ -28,7 +28,7 @@ catkin_package(
  INCLUDE_DIRS include
  LIBRARIES exploration_manager
  CATKIN_DEPENDS plan_env path_searching bspline bspline_opt active_perception traj_utils lkh_tsp_solver plan_manage
-#  DEPENDS system_lib
+        #  DEPENDS system_lib
 )
 
 include_directories(
@@ -45,7 +45,7 @@ add_executable(exploration_node
   src/fast_exploration_fsm.cpp
   src/fast_exploration_manager.cpp
   )
-target_link_libraries(exploration_node 
+target_link_libraries(exploration_node
   ${catkin_LIBRARIES}
   # -ldw
   )
@@ -66,4 +66,4 @@ target_link_libraries(write_tsp
 add_executable(load_obj test/load_obj.cpp)
 target_link_libraries(load_obj
   ${catkin_LIBRARIES} ${PCL_LIBRARIES}
-)
+)
diff --git a/fuel_planner/exploration_manager/include/exploration_manager/expl_data.h b/fuel_planner/exploration_manager/include/exploration_manager/expl_data.h
@@ -4,23 +4,27 @@
 #include <Eigen/Eigen>
 #include <vector>
 #include <bspline/Bspline.h>
+#include <geometry_msgs/Twist.h>
 
 using std::vector;
 using Eigen::Vector3d;
 
 namespace fast_planner {
 struct FSMData {
   // FSM data
-  bool trigger_, have_odom_, static_state_;
+  bool trigger_, have_odom_, static_state_, manual_;
   vector<string> state_str_;
 
-  Eigen::Vector3d odom_pos_, odom_vel_;  // odometry state
+  Eigen::Vector3d odom_pos_, odom_vel_, end_pt_, end_vel_;  // odometry state
   Eigen::Quaterniond odom_orient_;
   double odom_yaw_;
+	double end_yaw_;
 
   Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_;  // start state
   vector<Eigen::Vector3d> start_poss;
   bspline::Bspline newest_traj_;
+	vector<geometry_msgs::Twist> newest_velocities_;
+	vector<geometry_msgs::Twist> newest_accelerations_;
 };
 
 struct FSMParam {

diff --git a/fuel_planner/exploration_manager/include/exploration_manager/fast_exploration_fsm.h b/fuel_planner/exploration_manager/include/exploration_manager/fast_exploration_fsm.h
@@ -6,6 +6,7 @@
 #include <ros/ros.h>
 #include <nav_msgs/Path.h>
 #include <std_msgs/Empty.h>
+#include <std_msgs/String.h>
 #include <nav_msgs/Odometry.h>
 #include <visualization_msgs/Marker.h>
 
@@ -15,6 +16,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <trajectory_msgs/MultiDOFJointTrajectory.h>
 
 using Eigen::Vector3d;
 using std::vector;
@@ -29,12 +31,13 @@ class PlanningVisualization;
 struct FSMParam;
 struct FSMData;
 
-enum EXPL_STATE { INIT, WAIT_TRIGGER, PLAN_TRAJ, PUB_TRAJ, EXEC_TRAJ, FINISH };
+enum EXPL_STATE { INIT, WAIT_TRIGGER, PLAN_TRAJ, PLAN_MAN_TRAJ, MAN_GOAL_REACHED, PUB_TRAJ, EXEC_TRAJ, FINISH, PUB_360, PUB_FIRST_360};
 
 class FastExplorationFSM {
 private:
   /* planning utils */
   shared_ptr<FastPlannerManager> planner_manager_;
+	bool has_done_360_ = false;
   shared_ptr<FastExplorationManager> expl_manager_;
   shared_ptr<PlanningVisualization> visualization_;
 
@@ -47,21 +50,33 @@ class FastExplorationFSM {
   /* ROS utils */
   ros::NodeHandle node_;
   ros::Timer exec_timer_, safety_timer_, vis_timer_, frontier_timer_;
-  ros::Subscriber trigger_sub_, odom_sub_;
-  ros::Publisher replan_pub_, new_pub_, bspline_pub_;
+  ros::Subscriber trigger_sub_, odom_sub_, manual_goal_sub_;
+  ros::Publisher replan_pub_, new_pub_, bspline_pub_, trajectory_pub_, state_pub_, nmpc_pose_pub_;
+	bool is_first_command = true;
 
   /* helper functions */
   int callExplorationPlanner();
+	bool callKinodynamicReplan();
   void transitState(EXPL_STATE new_state, string pos_call);
+	void setStartTraj();
 
   /* ROS functions */
+	void setVisInfo(const ros::Time& time_r);
   void FSMCallback(const ros::TimerEvent& e);
   void safetyCallback(const ros::TimerEvent& e);
   void frontierCallback(const ros::TimerEvent& e);
   void triggerCallback(const nav_msgs::PathConstPtr& msg);
   void odometryCallback(const nav_msgs::OdometryConstPtr& msg);
+  void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);
+	void pubState();
   void visualize();
   void clearVisMarker();
+	bool hasMoved();
+	double find_tr();
+	Eigen::Vector3d old_odom_pos_;
+	Eigen::Quaterniond old_odom_orient_;
+
+	int try_count_ = 0;
 
 public:
   FastExplorationFSM(/* args */) {

diff --git a/fuel_planner/exploration_manager/include/exploration_manager/fast_exploration_manager.h b/fuel_planner/exploration_manager/include/exploration_manager/fast_exploration_manager.h
@@ -19,7 +19,7 @@ class FrontierFinder;
 struct ExplorationParam;
 struct ExplorationData;
 
-enum EXPL_RESULT { NO_FRONTIER, FAIL, SUCCEED };
+enum EXPL_RESULT { NO_FRONTIER, FAIL, SUCCEED, DO360 };
 
 class FastExplorationManager {
 public:
@@ -40,7 +40,8 @@ class FastExplorationManager {
   shared_ptr<FastPlannerManager> planner_manager_;
   shared_ptr<FrontierFinder> frontier_finder_;
   // unique_ptr<ViewFinder> view_finder_;
-
+  computePath(double next_yaw, Eigen::Vector3d yaw, Eigen::Vector3d pos, Eigen::Vector3d next_pos,
+                                      Eigen::Vector3d vel, Eigen::Vector3d acc, ros::Time t2);
 private:
   shared_ptr<EDTEnvironment> edt_environment_;
   shared_ptr<SDFMap> sdf_map_;
@@ -55,7 +56,7 @@ class FastExplorationManager {
                        vector<Vector3d>& refined_pts, vector<double>& refined_yaws);
 
   void shortenPath(vector<Vector3d>& path);
-
+	bool has_done_360_ = false;
 public:
   typedef shared_ptr<FastExplorationManager> Ptr;
 };

diff --git a/fuel_planner/exploration_manager/launch/algorithm.xml b/fuel_planner/exploration_manager/launch/algorithm.xml
@@ -25,21 +25,22 @@
 
   <!-- main node -->
   <node pkg="exploration_manager" name="exploration_node" type="exploration_node" output="screen">
-    <remap from ="/odom_world" to="$(arg odometry_topic)"/>
-    <remap from ="/map_ros/pose"   to = "$(arg sensor_pose_topic)"/> 
-    <remap from ="/map_ros/depth" to = "$(arg depth_topic)"/>
-    <remap from ="/map_ros/cloud" to="$(arg cloud_topic)"/>
+    <remap from ="~odom_world" to="$(arg odometry_topic)"/>
+    <remap from ="~map_ros/pose"   to = "$(arg sensor_pose_topic)"/>
+    <remap from ="~map_ros/depth" to = "$(arg depth_topic)"/>
+    <remap from ="~map_ros/cloud" to="$(arg cloud_topic)"/>
+    <remap from ="~waypoint_generator/waypoints" to="/waypoint_generator/waypoints"/>
 
     <param name="sdf_map/resolution"      value="0.1" /> 
-    <param name="sdf_map/map_size_x"   value="$(arg map_size_x_)" /> 
-    <param name="sdf_map/map_size_y"   value="$(arg map_size_y_)" /> 
-    <param name="sdf_map/map_size_z"   value="$(arg map_size_z_)" /> 
-    <!-- <param name="sdf_map/obstacles_inflation"     value="0.099" />  -->
-    <param name="sdf_map/obstacles_inflation"     value="0.199" /> 
+    <param name="sdf_map/map_size_x"   value="40" />
+    <param name="sdf_map/map_size_y"   value="20" />
+    <param name="sdf_map/map_size_z"   value="5" />
+    <param name="sdf_map/obstacles_inflation"     value="0.009" />
+<!--    <param name="sdf_map/obstacles_inflation"     value="0.199" /> -->
     <param name="sdf_map/local_bound_inflate"    value="0.5"/>
     <param name="sdf_map/local_map_margin" value="50"/>
     <param name="sdf_map/ground_height"        value="-1.0"/>
-    <param name="sdf_map/default_dist"        value="0.0"/>
+    <param name="sdf_map/default_dist"        value="5.0"/>
 
     <param name="sdf_map/p_hit"  value="0.65"/>
     <param name="sdf_map/p_miss" value="0.35"/>
@@ -68,17 +69,17 @@
     <param name="map_ros/k_depth_scaling_factor" value="1000.0"/>
     <param name="map_ros/skip_pixel" value="2"/>
     <param name="map_ros/esdf_slice_height" value="0.3"/>
-    <param name="map_ros/visualization_truncate_height"   value="10.09"/>
+    <param name="map_ros/visualization_truncate_height"   value="3.09"/>
     <param name="map_ros/visualization_truncate_low"   value="-2.0"/>
     <param name="map_ros/show_occ_time"  value="false"/>
     <param name="map_ros/show_esdf_time" value="false"/>
     <param name="map_ros/show_all_map" value="true"/>
     <param name="map_ros/frame_id"      value="world"/>
 
     <!-- Fsm -->
-    <param name="fsm/thresh_replan1" value="0.5" type="double"/>
+    <param name="fsm/thresh_replan1" value="0.2" type="double"/>
     <param name="fsm/thresh_replan2" value="0.5" type="double"/>
-    <param name="fsm/thresh_replan3" value="1.5" type="double"/>
+    <param name="fsm/thresh_replan3" value="0.7" type="double"/>
 
     <!-- <param name="fsm/thresh_replan1" value="0.00001" type="double"/> -->
     <!-- <param name="fsm/thresh_replan2" value="100.0" type="double"/> -->
@@ -100,7 +101,8 @@
     <param name="exploration/tsp_dir" value="$(find lkh_tsp_solver)/resource" type="string"/>
     <param name="exploration/relax_time" value="1.0" type="double"/>
 
-    <param name="frontier/cluster_min" value="100" type="int"/>
+
+    <param name="frontier/cluster_min" value="10" type="int"/>
     <param name="frontier/cluster_size_xy" value="2.0" type="double"/>
     <param name="frontier/cluster_size_z" value="10.0" type="double"/>
     <param name="frontier/min_candidate_dist" value="0.75" type="double"/>
@@ -129,30 +131,30 @@
     <param name="heading_planner/w" value="20000.0" type="double"/>
     <param name="heading_planner/weight_type" value="1" type="double"/>
 
-  <!-- planner manager -->
+    <!-- planner manager -->
     <!-- <param name="manager/max_vel" value="$(arg max_vel)" type="double"/> -->
     <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
     <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
     <param name="manager/max_jerk" value="4" type="double"/>
     <param name="manager/dynamic_environment" value="0" type="int"/>
     <param name="manager/local_segment_length" value="6.0" type="double"/>
     <param name="manager/clearance_threshold" value="0.2" type="double"/>
-    <param name="manager/control_points_distance" value="0.35" type="double"/>
+    <param name="manager/control_points_distance" value="0.5" type="double"/>
     <param name="manager/use_geometric_path" value="true" type="bool"/>
     <param name="manager/use_kinodynamic_path" value="true" type="bool"/>
     <param name="manager/use_topo_path" value="true" type="bool"/>
     <param name="manager/use_optimization" value="true" type="bool"/>
-    <param name="manager/use_active_perception" value="true" type="bool"/>
+    <param name="manager/use_active_perception" value="false" type="bool"/>
     <param name="manager/min_time" value="true" type="bool"/>
 
-  <!-- kinodynamic path searching -->
+    <!-- kinodynamic path searching -->
     <param name="search/max_tau" value="0.8" type="double"/>
     <param name="search/init_max_tau" value="1.0" type="double"/>
     <param name="search/max_vel" value="$(arg max_vel)" type="double"/>
     <param name="search/vel_margin" value="0.25" type="double"/>
     <param name="search/max_acc" value="$(arg max_acc)" type="double"/>
     <param name="search/w_time" value="10.0" type="double"/>
-    <param name="search/horizon" value="5.0" type="double"/>
+    <param name="search/horizon" value="7.0" type="double"/>
     <param name="search/lambda_heu" value="10.0" type="double"/>
     <param name="search/resolution_astar" value="0.025" type="double"/>
     <param name="search/time_resolution" value="0.8" type="double"/>

diff --git a/fuel_planner/exploration_manager/launch/exploration.launch b/fuel_planner/exploration_manager/launch/exploration.launch
@@ -6,7 +6,7 @@
 
   <arg name="init_x" value="0"/>
   <arg name="init_y" value="0"/>
-  <arg name="init_z" value="1.0"/> 
+  <arg name="init_z" value="1.0"/>
 
   <!-- topic of your odometry such as VIO or LIO -->
   <arg name="odom_topic" value="/state_ukf/odom" />
@@ -27,12 +27,12 @@
 
     <arg name="odometry_topic" value="$(arg odom_topic)"/>
     <!-- sensor pose: transform of camera frame in the world frame -->
-    <arg name="sensor_pose_topic" value="/pcl_render_node/sensor_pose"/>
+    <arg name="sensor_pose_topic" value="pcl_render_node/sensor_pose"/>
     <!-- depth topic: depth image, 640x480 by default -->
     <!-- cloud topic: point cloud measurement  -->
     <!-- subscribe ONLY TO ONE of the two topics -->
-    <arg name="depth_topic" value="/pcl_render_node/depth"/>
-    <arg name="cloud_topic" value="/pcl_render_node/cloud"/>
+    <arg name="depth_topic" value="pcl_render_node/depth"/>
+    <arg name="cloud_topic" value="pcl_render_node/cloud"/>
 
     <!-- intrinsic params of the depth camera -->
     <arg name="cx" value="321.04638671875"/>
@@ -45,9 +45,10 @@
     <arg name="max_acc" value="2.0" />
   </include>
 
+  <group ns="exploration_node">
   <!-- trajectory server -->
   <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
-    <remap from="/position_cmd" to="planning/pos_cmd"/>
+    <remap from="position_cmd" to="~planning/pos_cmd"/>
 
     <remap from="/odom_world" to="$(arg odom_topic)"/>
     <param name="traj_server/time_forward" value="1.5" type="double"/>
@@ -61,17 +62,18 @@
     <param name="perception_utils/right_angle" value="0.68901" type="double"/>
     <param name="perception_utils/max_dist" value="4.5" type="double"/>
     <param name="perception_utils/vis_dist" value="1.0" type="double"/>
-    
+
 
   </node>
 
   <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
-    <remap from="~odom" to="$(arg odom_topic)"/>        
+    <remap from="~odom" to="$(arg odom_topic)"/>
     <remap from="~goal" to="/move_base_simple/goal"/>
     <remap from="~traj_start_trigger" to="/traj_start_trigger" />
-    <param name="waypoint_type" value="point"/>    
+    <param name="waypoint_type" value="point"/>
   </node>
 
+  </group>
   <!-- use simulator -->
   <include file="$(find exploration_manager)/launch/simulator.xml">
     <arg name="map_size_x_" value="$(arg map_size_x)"/>
@@ -86,5 +88,4 @@
     <arg name="c_num" value="0"/>
     <arg name="p_num" value="130"/>
   </include>
-
 </launch>