Feature/waypoint flight

docker/Dockerfile

@@ -62,6 +62,9 @@ RUN wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz \
     && mkdir ceres-bin && cd ceres-bin \
     && cmake ../ceres-solver-2.1.0 && make -j3 && sudo make install
 
+# Install rospkg for rc_flight
+RUN pip3 install rospkg
+
 # Build livox_ros_driver
 RUN echo "source /opt/ros/melodic/setup.sh" >> /home/user/.bashrc
 WORKDIR /home/user

run_docker.sh

@@ -57,31 +57,16 @@ elif [ "$1" = "run" ]; then
         CMD_ARGS="$CMD_ARGS --gpus all"
     fi
 
-    # TODO(ulken94): The LIO-Livox should not be mounted for aarch64.
-    if [ "$ARCH" = "aarch64" ]; then
-        docker run $DOCKER_OPT --privileged \
-            -e DISPLAY=${DISPLAY} \
-            -e TERM=xterm-256color \
-            -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
-            -v /dev:/dev \
-            -v $PWD/src:/home/user/catkin_ws/src \
-            --network host \
-            $CMD_ARGS \
-            $DOCKER_TAG \
-            $RUN_SHELL
-    else
-        docker run $DOCKER_OPT --privileged \
-            -e DISPLAY=${DISPLAY} \
-            -e TERM=xterm-256color \
-            -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
-            -v /dev:/dev \
-            -v $PWD/catkin_ws/src:/home/user/catkin_ws/src \
-            -v $PWD/res:/home/user/res \
-            --network host \
-            $CMD_ARGS \
-            $DOCKER_TAG \
-            $RUN_SHELL
-    fi
+    docker run $DOCKER_OPT --privileged \
+        -e DISPLAY=${DISPLAY} \
+        -e TERM=xterm-256color \
+        -v /tmp/.X11-unix:/tmp/.X11-unix:ro \
+        -v /dev:/dev \
+        -v $PWD/src:/home/user/catkin_ws/src \
+        --network host \
+        $CMD_ARGS \
+        $DOCKER_TAG \
+        $RUN_SHELL
 
     last_cont_id=$(docker ps -qn 1)
     echo $(docker ps -qn 1) > $PWD/.last_exec_cont_id.txt

src/planner/plan_manage/include/plan_manage/ego_replan_fsm.h

@@ -115,6 +115,7 @@ namespace ego_planner
     bool is_need_replan_ = false;
     int reboundReplan_fail_count_ = 0;
     Eigen::Vector3d checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint);
+    Eigen::Vector3d checkEnableStartpoint(const Eigen::Vector3d &start);
 
   public:
     EGOReplanFSM(/* args */)

src/planner/plan_manage/launch/advanced_param.xml

@@ -106,7 +106,7 @@
     <param name="grid_map/min_ray_length" value="0.1"/>
     <param name="grid_map/max_ray_length" value="9.5"/>
 
-    <param name="grid_map/virtual_ceil_height"   value="4.0"/>
+    <param name="grid_map/virtual_ceil_height"   value="10.0"/>
     <param name="grid_map/visualization_truncate_height"   value="3.3"/>
     <param name="grid_map/show_occ_time"  value="false"/>
     <param name="grid_map/pose_type"     value="2"/>  

src/planner/plan_manage/launch/run_in_flight.launch

@@ -2,7 +2,16 @@
   <!-- size of map, change the size inflate x, y, z according to your application -->
   <arg name="map_size_x" value="150.0"/>
   <arg name="map_size_y" value="150.0"/>
-  <arg name="map_size_z" value=" 10.0"/>
+  <arg name="map_size_z" value=" 15.0"/>
+
+  <arg name="waypoint" default="$(find rc_demo)/res/waypoints.yaml" doc="Waypoint definition yaml file." />
+  <arg name="target_waypoint" default="waypoint_outdoor" doc="Target waypoint name to use defined in yaml file." />
+  <arg name="distance_tolerance" default="0.5" doc="Waypoint distance tolerance distance in meter." />
+  <arg name="angle_tolerance" default="10.0" doc="Waypoint angle tolerance degrees." />
+  <arg name="offset_x" default="0.0" doc="Offset x of the initial position" />
+  <arg name="offset_y" default="0.0" doc="Offset y of the initial position" />
+  <arg name="offset_z" default="0.0" doc="Offset z of the initial position" />
+  <arg name="offset_Y" default="0.0" doc="Offset yaw of the initial position" />
 
   <!-- topic of your odometry such as VIO or LIO -->
   <arg name="odom_topic" value="/Odometry" />
@@ -32,11 +41,11 @@
     <arg name="fy" value="387.229248046875"/>
 
     <!-- maximum velocity and acceleration the drone will reach -->
-    <arg name="max_vel" value="2.0" />
-    <arg name="max_acc" value="3.0" />
+    <arg name="max_vel" value="0.5" />
+    <arg name="max_acc" value="1.5" />
 
     <!--always set to 1.5 times grater than sensing horizen-->
-    <arg name="planning_horizon" value="7.5" /> 
+    <arg name="planning_horizon" value="14.5" /> 
 
     <!-- 1: use 2D Nav Goal to select goal  -->
     <!-- 2: use global waypoints below  -->
@@ -66,7 +75,7 @@
     <arg name="point4_y" value="0.0" />
     <arg name="point4_z" value="1.0" />
 
-    <arg name="control_zero_reset" value="true" />
+    <arg name="control_zero_reset" value="false" />
 
   </include>
 
@@ -76,8 +85,12 @@
 
     <remap from="/odom_world" to="$(arg odom_topic)"/>
     <param name="traj_server/time_forward" value="1.0" type="double"/>
-    <param name="traj_server/use_velocity_control" value="false" type="bool"/>
-    <param name="traj_server/enable_rotate_head" value="true" type="bool"/>
+    <param name="traj_server/use_velocity_control" value="true" type="bool"/>
+    <param name="traj_server/enable_rotate_head" value="false" type="bool"/>
+    <param name="traj_server/use_waypoint_yaw" value="true" type="bool"/>
+    <param name="traj_server/max_vel" value="0.5" type="double"/>
+    <param name="traj_server/z_offset" value="0.15" type="double"/>
+    <param name="traj_server/forward_length" value="0.6" type="double"/>
   </node>
 
   <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
@@ -88,8 +101,16 @@
     <param name="waypoint_type" value="manual-lonely-waypoint"/>    
   </node>
 
-  <node pkg="rc_demo" name="flight_control_node" type="rc_flight.py" output="screen">
-  </node>
+  <include file="$(find rc_demo)/launch/rc_flight.launch">
+    <arg name="rc_control" value="false" />
+    <arg name="waypoint" value="$(arg waypoint)" />
+    <arg name="target_waypoint" value="$(arg target_waypoint)" />
+    <arg name="distance_tolerance" value="$(arg distance_tolerance)" />
+    <arg name="offset_x" value="$(arg offset_x)" />
+    <arg name="offset_y" value="$(arg offset_y)" />
+    <arg name="offset_z" value="$(arg offset_z)" />
+    <arg name="offset_Y" value="$(arg offset_Y)" />
+  </include>
 
   <!-- use simulator -->
   <!-- <include file="$(find ego_planner)/launch/simulator.xml">
@@ -102,7 +123,6 @@
 
     <arg name="odometry_topic" value="$(arg odom_topic)" />
   </include> -->
-<!--
-  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/default.rviz" required="true" />
--->
+
+  <!-- node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/default.rviz" required="true" /-->
 </launch>

src/planner/plan_manage/launch/run_in_px4sim.launch

@@ -2,7 +2,7 @@
   <!-- size of map, change the size inflate x, y, z according to your application -->
   <arg name="map_size_x" value="100.0"/>
   <arg name="map_size_y" value="100.0"/>
-  <arg name="map_size_z" value=" 4.5"/>
+  <arg name="map_size_z" value=" 12.0"/>
 
   <!-- topic of your odometry such as VIO or LIO -->
   <arg name="odom_topic" value="/Odometry" />
@@ -78,6 +78,9 @@
     <param name="traj_server/time_forward" value="1.0" type="double"/>
     <param name="traj_server/use_velocity_control" value="true" type="bool"/>
     <param name="traj_server/enable_rotate_head" value="false" type="bool"/>
+    <param name="traj_server/use_waypoint_yaw" value="true" type="bool"/>
+    <param name="traj_server/max_vel" value="0.5" type="double"/>
+    <param name="traj_server/z_offset" value="0.15" type="double"/>
   </node>
 
   <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">

src/planner/plan_manage/src/ego_replan_fsm.cpp

@@ -243,14 +243,26 @@ namespace ego_planner
 
     cout << "Triggered!" << endl;
     trigger_ = true;
-    init_pt_ = odom_pos_;
+    init_pt_ = checkEnableStartpoint(odom_pos_);
 
-    bool success = false;
     end_pt_ << msg.poses[0].pose.position.x, msg.poses[0].pose.position.y, msg.poses[0].pose.position.z;
-    end_pt_ << checkEnableWaypoint(odom_pos_, end_pt_);
-    success = planner_manager_->planGlobalTraj(odom_pos_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+    visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 1.0, 0.25, 0.7), 0.3, 0);
 
-    visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0, 0.5, 0.5, 1), 0.3, 0);
+    Eigen::Vector3d sub_xy(end_pt_.x() - init_pt_.x(), end_pt_.y() - init_pt_.y(), 0);
+    if (sub_xy.norm() < no_replan_thresh_ && abs(end_pt_.z() - init_pt_.z()) > 0.1) {
+      traj_pts_.clear();
+      traj_pts_.push_back(init_pt_);
+      traj_pts_.push_back(end_pt_);
+      traj_pts_.push_back(end_pt_);
+      visualization_->displayTrajList(traj_pts_, 0);
+      visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1);
+      return;
+    }
+
+    end_pt_ << checkEnableWaypoint(init_pt_, end_pt_);
+    visualization_->displayGoalPoint(end_pt_, Eigen::Vector4d(0.25, 0.25, 1.0, 0.7), 0.3, 1);
+
+    bool success = planner_manager_->planGlobalTraj(init_pt_, odom_vel_, Eigen::Vector3d::Zero(), end_pt_, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
 
     if (success)
     {
@@ -375,7 +387,7 @@ namespace ego_planner
 
     case GEN_NEW_TRAJ:
     {
-      start_pt_ = odom_pos_;
+      start_pt_ = init_pt_;
       start_vel_ = odom_vel_;
       start_acc_.setZero();
       traj_pts_.clear();
@@ -449,6 +461,7 @@ namespace ego_planner
       {
         // cout << "near end" << endl;
         traj_pts_.push_back(end_pt_);
+        if (traj_pts_.size() == 2) traj_pts_.push_back(end_pt_);
         visualization_->displayTrajList(traj_pts_, 0);
         current_traj_ = generateTraj(traj_pts_);
         return;
@@ -589,45 +602,159 @@ namespace ego_planner
     // }
   }
 
-  Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &waypoint) {
+  Eigen::Vector3d EGOReplanFSM::checkEnableStartpoint(const Eigen::Vector3d &start) {
+    // first, check is waypoint validate
+    auto map = planner_manager_->grid_map_;
+    auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector<Eigen::Vector3d>& pts) {
+      pts.clear();
+      double res = map->getResolution();
+      /* ---------- all inflate ---------- */
+      for (int x = -step; x <= step; ++x)
+        for (int y = -step; y <= step; ++y)
+          for (int z = -step; z <= step; ++z) {
+            if (sqrt(x*x + y*y + z*z) > (double)step) continue;
+            Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res);
+            if (!map->getInflateOccupancy(new_pt)) {
+              pts.push_back(new_pt);
+            }
+          }
+    };
+
+    int inf_step = 2; // TODO: add inf_step parameter for this function
+    vector<Eigen::Vector3d> inf_pts;
+    inflatePointWithoutObs(start, inf_step, inf_pts);
+
+    if (inf_pts.size() > 0) {
+      double best_score = inf_pts.size();
+      Eigen::Vector3d best_pt = start;
+
+      for (auto inf_pt : inf_pts) {
+        vector<Eigen::Vector3d> inf_pts_sub;
+        inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub);
+        double score = (double)inf_pts_sub.size();
+        if (score > best_score) {
+          best_pt = inf_pt;
+          best_score = score;
+        }
+      }
+
+      return best_pt;
+    }
+
+    return start;
+  }
+
+  Eigen::Vector3d EGOReplanFSM::checkEnableWaypoint(const Eigen::Vector3d &start, const Eigen::Vector3d &end) {
+    Eigen::Vector3d waypoint = end;
+
 ros::Time time_1 = ros::Time::now();
+    // first, check is waypoint validate
+    auto map = planner_manager_->grid_map_;
+    auto inflatePointWithoutObs = [map](const Eigen::Vector3d& pt, int step, vector<Eigen::Vector3d>& pts) {
+      pts.clear();
+      double res = map->getResolution();
+      /* ---------- all inflate ---------- */
+      for (int x = -step; x <= step; ++x)
+        for (int y = -step; y <= step; ++y)
+          for (int z = -step/2; z <= step/2; ++z) {
+            if (sqrt(x*x + y*y + z*z) > (double)step) continue;
+            Eigen::Vector3d new_pt(pt(0) + (double)x * res, pt(1) + (double)y * res, pt(2) + (double)z * res);
+            if (!map->getInflateOccupancy(new_pt)) {
+              pts.push_back(new_pt);
+            }
+          }
+    };
+
+    int inf_step = 7; // TODO: add inf_step parameter for this function
+    vector<Eigen::Vector3d> inf_pts;
+    inflatePointWithoutObs(waypoint, inf_step, inf_pts);
+
+    if (inf_pts.size() > 0) {
+      double best_score = inf_pts.size();
+      Eigen::Vector3d best_pt = waypoint;
+
+      for (auto inf_pt : inf_pts) {
+        vector<Eigen::Vector3d> inf_pts_sub;
+        inflatePointWithoutObs(inf_pt, inf_step, inf_pts_sub);
+        double score = (double)inf_pts_sub.size();
+        if (score > best_score) {
+          best_pt = inf_pt;
+          best_score = score;
+        }
+      }
+
+      waypoint = best_pt;
+    }
+
+    // TODO: check A* start path
+ros::Time time_2 = ros::Time::now();
     vector<vector<Eigen::Vector3d>> a_star_pathes;
     vector<Eigen::Vector3d> a_star_path_H, a_star_path_V, a_star_path_A;
     double expected_length = (waypoint - start).norm() / planner_manager_->grid_map_->getResolution();
+
+    auto shrink_path = [&](vector<Eigen::Vector3d> a_star_path){
+      if (a_star_path.size() < 2) return a_star_path;
+
+      vector<Eigen::Vector3d> shrinked_path;
+      Eigen::Vector3d last_ptd = a_star_path.front();
+      Eigen::Vector3i last_pti;
+      Eigen::Vector3i last_dir(0, 0, 0);
+      planner_manager_->grid_map_->posToIndex(last_ptd, last_pti);
+
+      for (size_t i = 0; i < a_star_path.size(); i++) {
+        Eigen::Vector3i cur_pti;
+        planner_manager_->grid_map_->posToIndex(a_star_path[i], cur_pti);
+        Eigen::Vector3i cur_dir = cur_pti - last_pti;
+        if (last_dir != cur_dir) {
+          shrinked_path.push_back(last_ptd);
+          last_ptd = a_star_path[i-1];
+          last_pti = cur_pti;
+          last_dir = cur_dir;
+        } else if (i == a_star_path.size() - 1) {
+          shrinked_path.push_back(last_ptd);
+        } else {
+          last_pti = cur_pti;
+        }
+      }
+
+      shrinked_path.push_back(a_star_path.back());
+      return shrinked_path;
+    };
 
     if (expected_length > 1) {
       planner_manager_->grid_map_->setSearchAreaH();
-      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
         a_star_path_H = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
       }
     }
 
-    if (a_star_path_H.size() > expected_length * 1.1) {
-      planner_manager_->grid_map_->setSearchAreaV(end_pt_);
-      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+    if (a_star_path_H.size() > expected_length * 1.1 || a_star_path_H.size() == 0) {
+      planner_manager_->grid_map_->setSearchAreaV(waypoint);
+      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
         a_star_path_V = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
       }
     }
 
-    if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1))
+    if (a_star_path_V.size() > expected_length * 1.1 || (a_star_path_V.size() == 0 && a_star_path_H.size() > expected_length * 1.1) || a_star_path_H.size() == 0)
     {
       planner_manager_->grid_map_->resetSearchArea();
-      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, odom_pos_, end_pt_)) {
+      if (planner_manager_->bspline_optimizer_rebound_->a_star_->AstarSearch(0.1, start, waypoint)) {
         a_star_path_A = planner_manager_->bspline_optimizer_rebound_->a_star_->getPath();
       }
     }
 
-    a_star_pathes.push_back(a_star_path_H);
-    a_star_pathes.push_back(a_star_path_V);
-    a_star_pathes.push_back(a_star_path_A);
+    a_star_pathes.push_back(shrink_path(a_star_path_H));
+    a_star_pathes.push_back(shrink_path(a_star_path_V));
+    a_star_pathes.push_back(shrink_path(a_star_path_A));
 
     planner_manager_->visualization_->displayAStarList(a_star_pathes, 0);
     planner_manager_->grid_map_->resetSearchArea();
 
-ros::Time time_2 = ros::Time::now();
+ros::Time time_3 = ros::Time::now();
 // compute time check
-std::cout << "AstarSearch time : " << (time_2 - time_1).toSec() << std::endl;
-std::cout << "expected : " << (waypoint - start).norm() << ", ";
+std::cout << "move goal time : " << (time_2 - time_1).toSec() << std::endl;
+std::cout << "AstarSearch time : " << (time_3 - time_2).toSec() << std::endl;
+std::cout << "expected : " << expected_length << ", ";
 std::cout << "H : " << a_star_path_H.size() << ", ";
 std::cout << "V : " << a_star_path_V.size() << ", ";
 std::cout << "A : " << a_star_path_A.size() << std::endl;