Local Planner Disparity Packages

castacks · Jul 24, 2024 · cdc0520 · cdc0520
1 parent e4a94d9
commit cdc0520
Show file tree

Hide file tree

Showing 28 changed files with 4,074 additions and 0 deletions.
diff --git a/ros_ws/src/robot/autonomy/planning/local/core_disparity_map_representation b/ros_ws/src/robot/autonomy/planning/local/core_disparity_map_representation
diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/CMakeLists.txt
@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(core_local_planner)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  nav_msgs
+  core_trajectory_library
+  core_trajectory_controller
+  core_map_representation_interface
+  tflib
+  actionlib_msgs
+  behavior_tree
+  core_trajectory_msgs
+  pluginlib
+)
+
+generate_messages(
+  DEPENDENCIES actionlib_msgs std_msgs
+)
+
+find_package(base)
+
+catkin_package(
+  CATKIN_DEPENDS actionlib_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${base_INCLUDE_DIRS}
+  include
+)
+
+add_executable(local_planner src/local_planner.cpp)
+
+add_dependencies(local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${base_EXPORTED_TARGETS})
+
+target_link_libraries(local_planner
+  ${catkin_LIBRARIES}
+  ${base_LIBRARIES}
+)
diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/README.md b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/README.md
@@ -0,0 +1,4 @@
+### LocalPlanner
+
+
+Author:
diff --git a/...bot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h b/...bot/autonomy/planning/local/core_local_planner/include/core_local_planner/local_planner.h
@@ -0,0 +1,109 @@
+#ifndef _LOCAL_PLANNER_H_
+#define _LOCAL_PLANNER_H_
+
+#include <base/BaseNode.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Range.h>
+#include <string>
+#include <vector>
+#include <list>
+#include <core_trajectory_library/trajectory_library.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Int32.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+//#include <disparity_graph/disparity_graph.h>
+//#include <disparity_map_representation/disparity_map_representation.h>
+//#include <pointcloud_map_representation/pointcloud_map_representation.h>
+#include <core_map_representation_interface/map_representation.h>
+//#include <disparity_map_representation/disparity_map_representation.h>
+#include <tf/transform_listener.h>
+#include <core_trajectory_controller/TrajectoryMode.h>
+
+#include <actionlib/server/simple_action_server.h>
+#include <behavior_tree/behavior_tree.h>
+
+#include <pluginlib/class_loader.h>
+
+class LocalPlanner : public BaseNode {
+private:
+  TrajectoryLibrary* traj_lib;
+
+  std::string map_representation;
+  bool got_global_plan;
+  core_trajectory_msgs::TrajectoryXYZVYaw global_plan;
+  double global_plan_trajectory_distance;
+  bool got_look_ahead, got_tracking_point;
+  nav_msgs::Odometry look_ahead_odom, tracking_point_odom;
+
+  std::vector<Trajectory> static_trajectories;
+
+  double waypoint_spacing, obstacle_check_radius, obstacle_penalty_weight, forward_progress_penalty_weight;
+  double robot_radius;
+  int obstacle_check_points;
+
+  double look_past_distance;
+
+  float waypoint_buffer_duration, waypoint_spacing_threshold, waypoint_angle_threshold;
+  std::list<geometry_msgs::PointStamped> waypoint_buffer;
+
+  // bool use_fixed_height;
+  const int GLOBAL_PLAN_HEIGHT = 0;
+  const int FIXED_HEIGHT = 1;
+  const int RANGE_SENSOR_HEIGHT = 2;
+  int height_mode;
+  double height_above_ground;
+  double fixed_height;
+  bool got_range_up, got_range_down;
+  sensor_msgs::Range range_up, range_down;
+
+  const int TRAJECTORY_YAW = 0;
+  const int SMOOTH_YAW = 1;
+  int yaw_mode;
+
+
+  // custom waypoint params
+  enum GoalMode {CUSTOM_WAYPOINT, AUTO_WAYPOINT, TRAJECTORY};
+  GoalMode goal_mode;
+  double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold;
+
+  //MapRepresentationDeprecated* map;
+  //MapRepresentation* pc_map;
+  boost::shared_ptr<MapRepresentation> pc_map;
+
+  // subscribers
+  ros::Subscriber global_plan_sub, waypoint_sub, look_ahead_sub, tracking_point_sub, range_up_sub, range_down_sub, custom_waypoint_sub;
+  tf::TransformListener* listener;
+
+  // publishers
+  ros::Publisher vis_pub, traj_pub, traj_track_pub, obst_vis_pub, global_plan_vis_pub, look_past_vis_pub;
+
+  // services
+  ros::ServiceClient traj_mode_client;
+
+  bool get_best_trajectory(std::vector<Trajectory> trajs,
+			   Trajectory global_plan, Trajectory* best_traj);
+  void update_waypoint_mode();
+
+public:
+  LocalPlanner(std::string node_name);
+
+  virtual bool initialize();
+  virtual bool execute();
+  virtual ~LocalPlanner();
+
+  // subscriber callbacks
+  void global_plan_callback(core_trajectory_msgs::TrajectoryXYZVYaw global_plan);
+  void waypoint_callback(geometry_msgs::PointStamped wp);
+  void custom_waypoint_callback(geometry_msgs::PoseStamped wp);
+  void look_ahead_callback(nav_msgs::Odometry odom);
+  void tracking_point_callback(nav_msgs::Odometry odom);
+  void range_up_callback(sensor_msgs::Range range);
+  void range_down_callback(sensor_msgs::Range range);
+};
+
+
+#endif
diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner.launch
@@ -0,0 +1,83 @@
+<?xml version="1.0" encoding="utf-8"?>
+<launch>
+
+  <arg name="robot_name" default="uav1" />
+  <arg name="map_representation" default="PointCloudMapRepresentation" />
+
+  <node name="local_planner" pkg="core_local_planner" type="local_planner" output="screen">
+
+    <param name="execute_target" type="double" value="5" />
+
+    <param name="map_representation" value="$(arg map_representation)" />
+
+    <!-- Trajectory parameters -->
+    <param name="trajectory_library_config" type="string" value="$(find core_trajectory_library)/config/acceleration_magnitudes.yaml" />
+    <param name="dt" value="0.2" />
+    <param name="ht" value="2.4" />
+    <param name="max_velocity" value="0.7" />
+    <param name="magnitude" value="0.5" />
+    <param name="tf_prefix" value="$(arg robot_name)" />
+
+    <param name="waypoint_spacing" type="double" value="0.2" />
+    <param name="obstacle_check_radius" type="double" value="2." />
+    <param name="obstacle_check_points" type="int" value="5" />
+    <param name="obstacle_penalty_weight" type="double" value="0." />
+    <param name="forward_progress_penalty_weight" type="double" value="5.0" />
+    <param name="look_past_distance" type="double" value="0" />
+    <!-- <param name="use_fixed_height" type="bool" value="true" /> -->
+    <!-- 0: use height from global plan, 1: use a fixed height, 2: use range sensors for height above ground setpoint -->
+    <param name="height_mode" type="int" value="1" />
+    <param name="height_above_ground" type="double" value="1.0" />
+    <param name="fixed_height" type="double" value="2.0" />
+
+    <param name="waypoint_buffer_duration" type="double" value="35." />
+    <param name="waypoint_spacing_threshold" type="double" value="0.5" />
+    <param name="waypoint_angle_threshold" type="double" value="30." />
+
+    <!-- takeoff/landing parameters -->
+    <param name="takeoff_height" type="double" value="0.9" />
+    <param name="takeoff_velocity" type="double" value="0.2" />
+    <param name="takeoff_acceptance_distance" type="double" value="0.3" />
+    <param name="takeoff_acceptance_time" type="double" value="1.0" />
+    <param name="landing_stationary_distance_threshold" type="double" value="0.05" />
+    <param name="landing_acceptance_time" type="double" value="5.0" />
+    <param name="safe_spot_target_frame" type="string" value="map" />
+    <param name="safe_spot_comms_timeout" type="double" value="2." />
+    <param name="safe_spot_landing_point_history" type="int" value="3" />
+    <param name="safe_spot_tracking_point_tolerance" type="double" value="0.05" />
+
+    <!-- custom waypoint parameters -->
+    <param name="custom_waypoint_timeout_factor" type="double" value="0.3" />
+    <param name="custom_waypoint_distance_threshold" type="double" value="0.5" />
+
+    <!-- 0: use the yaw from the subscribed trajectory, 1: smoothly vary the yaw in the direction of the subscribed trajectory -->
+    <param name="yaw_mode" type="int" value="1" />
+
+    <param name="robot_radius" type="double" value="1.0" />
+
+    <param name="pointcloud_map/node_spacing" type="double" value="0.4" />
+    <param name="pointcloud_map/node_limit" type="int" value="10" />
+    <param name="pointcloud_map/node_decay_time" type="double" value="15." />
+    <param name="pointcloud_map/target_frame" type="string" value="$(arg robot_name)/map" />
+    <param name="pointcloud_map/lidar_frame" type="string" value="$(arg robot_name)/velodyne" />
+    <param name="pointcloud_map/lidar_vertical_fov" type="double" value="14." />
+    <param name="pointcloud_map/robot_radius" type="double" value="1.0" />
+    <param name="pointcloud_map/obstacle_check_radius" type="double" value="2." />
+    <param name="pointcloud_map/obstacle_check_points" type="int" value="0" />
+
+
+    <param name="disparity_map/obstacle_check_points" type="int" value="4" />
+    <param name="disparity_map/obstacle_check_radius" type="double" value="1." />
+
+    <remap from="uav1/points" to="velodyne_points" />
+    <remap from="range_up" to="/uav1/teraranger_one_up" />
+    <remap from="range_down" to="/uav1/teraranger_one_down" />
+    <remap from="right/camera_info" to="front_stereo/right/camera_info" />
+    <remap from="left/disparity_expanded_fg" to="front_stereo/left/disparity_expanded_fg" />
+    <remap from="left/disparity_expanded_bg" to="front_stereo/left/disparity_expanded_bg" />
+  </node>
+
+  <!-- <node name="test_global_planner" pkg="local_planner" type="test_global_planner.py" /> -->
+  <!-- <node name="pose_to_path" pkg="local_planner" type="pose_to_path.py" /> -->
+
+</launch>
diff --git a/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner_bag.launch b/ros_ws/src/robot/autonomy/planning/local/core_local_planner/launch/local_planner_bag.launch
@@ -0,0 +1,59 @@
+<launch>
+
+  <node name="local_planner_drone" pkg="local_planner_drone" type="local_planner" output="screen">
+    <param name="execute_target" type="double" value="5" />
+
+    <remap from="/local_planner_global_plan_vis" to="/local_planner_global_plan_vis_debug" />
+
+    <param name="trajectory_library_config" type="string" value="$(find trajectory_library)/config/acceleration_magnitudes.yaml" />
+
+    <param name="waypoint_spacing" type="double" value="0.2" />
+    <param name="obstacle_check_radius" type="double" value="2." />
+    <param name="obstacle_check_points" type="int" value="5" />
+    <param name="obstacle_penalty_weight" type="double" value="0." />
+    <param name="forward_progress_penalty_weight" type="double" value="5.0" />
+    <param name="look_past_distance" type="double" value="0" />
+    <!-- <param name="use_fixed_height" type="bool" value="true" /> -->
+    <!-- 0: use height from global plan, 1: use a fixed height, 2: use range sensors for height above ground setpoint -->
+    <param name="height_mode" type="int" value="2" />
+    <param name="height_above_ground" type="double" value="1.0" />
+    <param name="fixed_height" type="double" value="1.0" />
+
+    <param name="waypoint_buffer_duration" type="double" value="35." />
+
+    <!-- takeoff/landing parameters -->
+    <param name="takeoff_height" type="double" value="1.0" />
+    <param name="takeoff_velocity" type="double" value="0.2" />
+    <param name="takeoff_acceptance_distance" type="double" value="0.3" />
+    <param name="takeoff_acceptance_time" type="double" value="1.0" />
+    <param name="landing_stationary_distance_threshold" type="double" value="0.05" />
+    <param name="landing_acceptance_time" type="double" value="5.0" />
+    <param name="safe_spot_target_frame" type="string" value="map" />
+    <param name="safe_spot_comms_timeout" type="double" value="2." />
+    <param name="safe_spot_landing_point_history" type="int" value="3" />
+    <param name="safe_spot_tracking_point_tolerance" type="double" value="0.05" />
+
+    <!-- 0: use the yaw from the subscribed trajectory, 1: smoothly vary the yaw in the direction of the subscribed trajectory -->
+    <param name="yaw_mode" type="int" value="1" />
+
+    <param name="robot_radius" type="double" value="0.35" />
+
+    <param name="pointcloud_map/node_spacing" type="double" value="1.0" />
+    <param name="pointcloud_map/node_limit" type="int" value="5" />
+    <param name="pointcloud_map/target_frame" type="string" value="map" />
+    <param name="pointcloud_map/lidar_frame" type="string" value="/velodyne" />
+    <param name="pointcloud_map/lidar_vertical_fov" type="double" value="30." />
+    <param name="pointcloud_map/robot_radius" type="double" value="0.6" />
+    <param name="pointcloud_map/obstacle_check_radius" type="double" value="2." />
+    <param name="pointcloud_map/obstacle_check_points" type="int" value="5" />
+
+    <remap from="/uav1/points" to="/velodyne_cloud_registered" />
+    <remap from="/range_up" to="/uav1/teraranger_one_up" />
+    <remap from="/range_down" to="/uav1/teraranger_one_down" />
+    <remap from="/uav1/points" to="/velodyne_cloud_registered" />
+  </node>
+
+  <!-- <node name="test_global_planner" pkg="local_planner" type="test_global_planner.py" /> -->
+  <!-- <node name="pose_to_path" pkg="local_planner" type="pose_to_path.py" /> -->
+
+</launch>
diff --git a/...rc/robot/autonomy/planning/local/core_local_planner/launch/local_planner_disparity.launch b/...rc/robot/autonomy/planning/local/core_local_planner/launch/local_planner_disparity.launch
@@ -0,0 +1,85 @@
+<launch>
+
+  <node name="local_planner" pkg="local_planner" type="local_planner" output="screen">
+    <param name="execute_target" type="double" value="5" />
+
+    <param name="trajectory_library_config" type="string" value="$(find trajectory_library)/config/acceleration_magnitudes.yaml" />
+
+    <param name="waypoint_spacing" type="double" value="0.2" />
+    <param name="obstacle_check_radius" type="double" value="2." />
+    <param name="obstacle_check_points" type="int" value="5" />
+    <param name="obstacle_penalty_weight" type="double" value="0." />
+    <param name="forward_progress_penalty_weight" type="double" value="1.0" />
+    <param name="look_past_distance" type="double" value="0" />
+    <!-- <param name="use_fixed_height" type="bool" value="true" /> -->
+    <!-- 0: use height from global plan, 1: use a fixed height, 2: use range sensors for height above ground setpoint -->
+    <param name="height_mode" type="int" value="2" />
+    <param name="height_above_ground" type="double" value="1.0" />
+    <param name="fixed_height" type="double" value="-18.5" />
+
+    <!-- 0: use the yaw from the subscribed trajectory, 1: smoothly vary the yaw in the direction of the subscribed trajectory -->
+    <param name="yaw_mode" type="int" value="1" />
+
+    <param name="robot_radius" type="double" value="0.6" />
+
+    <param name="pointcloud_map/node_spacing" type="double" value="1.0" />
+    <param name="pointcloud_map/node_limit" type="int" value="5" />
+    <param name="pointcloud_map/target_frame" type="string" value="map" />
+    <param name="pointcloud_map/lidar_frame" type="string" value="/uav1/front_laser" />
+    <param name="pointcloud_map/lidar_vertical_fov" type="double" value="30." />
+    <param name="pointcloud_map/robot_radius" type="double" value="0.6" />
+    <param name="pointcloud_map/obstacle_check_radius" type="double" value="2." />
+    <param name="pointcloud_map/obstacle_check_points" type="int" value="5" />
+
+    <remap from="/range_up" to="/uav1/teraranger_one_up" />
+    <remap from="/range_down" to="/uav1/teraranger_one_down" />
+
+    <remap from="/ceye/left/expanded_disparity_fg" to="/uav1/camera/front/disparity_fg" />
+    <remap from="/ceye/left/expanded_disparity_bg" to="/uav1/camera/front/disparity_bg" />
+    <remap from="/nerian_sp1/right/camera_info" to="/uav1/camera/front/right/camera_info" />
+
+    <remap from="/foreground_disparity" to ="/uav1/camera/front/disparity_fg" />
+    <remap from="/background_disparity" to ="/uav1/camera/front/disparity_bg" />
+    <remap from="/camera_info" to="/uav1/camera/front/right/camera_info" />
+  </node>
+
+  <!-- <node name="test_global_planner" pkg="local_planner" type="test_global_planner.py" /> -->
+  <!-- <node name="pose_to_path" pkg="local_planner" type="pose_to_path.py" /> -->
+
+  <!-- Disparity -->
+  <group ns="/uav1/camera/front">
+    <node pkg="stereo_image_proc" type="stereo_image_proc" name="front_stereo_image_proc" output="screen" />
+  </group>
+
+  <group ns="/uav1/camera/back">
+    <node pkg="stereo_image_proc" type="stereo_image_proc" name="front_stereo_image_proc" output="screen" />
+  </group>
+
+  <group ns="/uav1/camera/left">
+    <node pkg="stereo_image_proc" type="stereo_image_proc" name="front_stereo_image_proc" output="screen" />
+  </group>
+
+  <group ns="/uav1/camera/right">
+    <node pkg="stereo_image_proc" type="stereo_image_proc" name="front_stereo_image_proc" output="screen" />
+  </group>
+
+  <group ns="/uav1/camera/top">
+    <node pkg="stereo_image_proc" type="stereo_image_proc" name="front_stereo_image_proc" output="screen" />
+  </group>
+
+
+  <!-- Obstacle Detection -->
+  <node pkg="disparity_expansion" type="disparity_expansion" name="front_disparity_expansion" output="screen" >
+    <param name="lut_max_disparity" type="int" value="100" />
+    <param name="robot_radius" type="double" value=".25" />
+    <param name="bg_multiplier" type="double" value="2.0" />
+    <param name="padding" type="double" value="-1" />
+    <param name="sensor_pixel_error" type="double" value="0.2" />
+    <remap from="/nerian_sp1/right/camera_info" to="/uav1/camera/front/right/camera_info" />
+    <remap from="/nerian_sp1/disparity_map_32F" to="/uav1/camera/front/disparity" />
+
+    <remap from="/ceye/left/expanded_disparity_fg" to="/uav1/camera/front/disparity_fg" />
+    <remap from="/ceye/left/expanded_disparity_bg" to="/uav1/camera/front/disparity_bg" />
+  </node>
+
+</launch>