sravan/Fast-RRT-Star
1
0
Fork 0
mirror of https://github.com/mitre/Fast-RRT-Star.git synced 2025-09-03 10:03:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Repo Re-Org, More like ROS Package

Browse Source 
- Reorganize repo so docker stuff is
  a folder within fast_rrt_ros ROS package
- Update filepaths in README
- Update `build_and_run_docker.bash`
  to use main folder as build context,
  but use Dockerfile inside docker folder
- Update filepaths in Dockerfile
This commit is contained in:
Sravan Balaji
2023-01-26 16:10:58 -05:00
parent 3d59c49b49
commit 1ab700f27f
33 changed files with 27 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
docker/Dockerfile
View File

@@ -18,11 +18,11 @@ RUN apt-get update && \
# Copy launch files and husky description to appropriate locations
ARG HUSKY_NAV_LAUNCH=husky_navigation/launch
ARG HUSKY_DES_URDF=husky_description/urdf
COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch
COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch
COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch
COPY ./$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch
COPY ./$HUSKY_DES_URDF/husky.urdf.xacro /opt/ros/$ROS_DISTRO/share/$HUSKY_DES_URDF/husky.urdf.xacro
COPY ./docker/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star.launch
COPY ./docker/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_debug.launch
COPY ./docker/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo.launch
COPY ./docker/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch /opt/ros/$ROS_DISTRO/share/$HUSKY_NAV_LAUNCH/move_base_fast_rrt_star_mapless_demo_debug.launch
COPY ./docker/$HUSKY_DES_URDF/husky.urdf.xacro /opt/ros/$ROS_DISTRO/share/$HUSKY_DES_URDF/husky.urdf.xacro
# Declare some environment variables
ENV ROS_WORKSPACE=/root/catkin_ws
@@ -31,15 +31,18 @@ ENV ROS_PACKAGE_PATH=/root/catkin_ws/src:/opt/ros/$ROS_DISTRO/share
# Add setup.bash sourcing to bashrc
RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> /etc/bash.bashrc
# Place Fast-RRT* in catkin workspace
COPY ./fast_rrt_ros/ /root/catkin_ws/src/fast_rrt_ros
COPY ./CMakeLists.txt /root/catkin_ws/src/CMakeLists.txt
# Copy catkin_ws CMakeLists, scripts, and planner configuration to image
WORKDIR /root/catkin_ws/
COPY ./docker/CMakeLists.txt ./src/CMakeLists.txt
COPY ./scripts/ ./scripts
COPY ./config/planner.yaml /opt/ros/$ROS_DISTRO/share/husky_navigation/config/planner.yaml
# Copy scripts to image
COPY ./scripts/ /root/catkin_ws/scripts/
# Copy planner configuration
COPY ./fast_rrt_ros/config/planner.yaml /opt/ros/$ROS_DISTRO/share/husky_navigation/config/planner.yaml
# Set working directory
WORKDIR /root/catkin_ws/
# Copy Fast-RRT* to catkin workspace in image
WORKDIR /root/catkin_ws/src/fast_rrt_ros
COPY ./include/ ./include/
COPY ./src/ ./src/
COPY ./CMakeLists.txt ./CMakeLists.txt
COPY ./fast_rrt_star_planner_plugin.xml ./fast_rrt_star_planner_plugin.xml
COPY ./LICENSE ./LICENSE
COPY ./package.xml ./package.xml
COPY ./README.md ./README.md

32
docker/build_and_run_docker.bash Executable file
View File

@@ -0,0 +1,32 @@
#!/bin/bash
# Linux X server setup
XAUTH=/tmp/docker.xauth
if [ ! -f $XAUTH ]
then
xauth_list=$(xauth nlist :0 | sed -e 's/^..../ffff/')
if [ ! -z "$xauth_list" ]
then
echo $xauth_list | xauth -f $XAUTH nmerge -
else
touch $XAUTH
fi
chmod a+r $XAUTH
fi
# Build husky_image from Dockerfile in this directory
docker build -t husky_image -f ./docker/Dockerfile .
# Run husky_image with specified environment variables, volume mounting, and host networking
docker run \
-it \
--env="DISPLAY=$DISPLAY" \
--env="LIBGL_ALWAYS_INDIRECT=$LIBGL_ALWAYS_INDIRECT" \
--env="QT_X11_NO_MITSHM=1" \
--env="XAUTHORITY=$XAUTH" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--volume="$XAUTH:$XAUTH" \
--privileged \
--network=host \
husky_image \
bash

24
docker/fast_rrt_ros/CMakeLists.txt
View File

@@ -1,24 +0,0 @@
cmake_minimum_required(VERSION 3.0.2)
project(fast_rrt_ros)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
costmap_2d
nav_core
base_local_planner
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS roscpp rospy std_msgs costmap_2d nav_core base_local_planner
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(fast_rrt_star_planner_lib src/fast_rrt_star_planner.cpp src/fast_rrt_star_node.cpp src/fast_rrt_star_helper.cpp)
target_link_libraries(fast_rrt_star_planner_lib ${catkin_LIBRARIES})

103
docker/fast_rrt_ros/config/planner.yaml
View File

@@ -1,103 +0,0 @@
controller_frequency: 5.0
recovery_behavior_enabled: true
NavfnROS:
allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
default_tolerance: 0.1 # A tolerance on the goal point for the planner.
TrajectoryPlannerROS:
# Robot Configuration Parameters
acc_lim_x: 2.5
acc_lim_theta: 3.2
max_vel_x: 1.0
min_vel_x: 0.0
max_vel_theta: 1.0
min_vel_theta: -1.0
min_in_place_vel_theta: 0.2
holonomic_robot: false
escape_vel: -0.1
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.1
xy_goal_tolerance: 0.2
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0
sim_granularity: 0.02
angular_sim_granularity: 0.02
vx_samples: 6
vtheta_samples: 20
controller_frequency: 20.0
# Trajectory scoring parameters
meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
occdist_scale: 0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
pdist_scale: 0.75 # The weighting for how much the controller should stay close to the path it was given . default 0.6
gdist_scale: 1.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed default 0.8
heading_lookahead: 0.325 #How far to look ahead in meters when scoring different in-place-rotation trajectories
heading_scoring: false #Whether to score based on the robot's heading to the path or its distance from the path. default false
heading_scoring_timestep: 0.8 #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
simple_attractor: false
publish_cost_grid_pc: true
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
escape_reset_dist: 0.1
escape_reset_theta: 0.1
DWAPlannerROS:
# Robot configuration parameters
acc_lim_x: 2.5
acc_lim_y: 0
acc_lim_th: 3.2
max_vel_x: 0.5
min_vel_x: 0.0
max_vel_y: 0
min_vel_y: 0
max_trans_vel: 0.5
min_trans_vel: 0.1
max_rot_vel: 1.0
min_rot_vel: 0.2
# Goal Tolerance Parameters
yaw_goal_tolerance: 0.1
xy_goal_tolerance: 0.2
latch_xy_goal_tolerance: false
# # Forward Simulation Parameters
# sim_time: 2.0
# sim_granularity: 0.02
# vx_samples: 6
# vy_samples: 0
# vtheta_samples: 20
# penalize_negative_x: true
# # Trajectory scoring parameters
# path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given
# goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed
# occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles
# forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters
# stop_time_buffer: 0.2 # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds
# scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s
# max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by
# # Oscillation Prevention Parameters
# oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
Fast_RRTStarPlanner:
max_num_iter: 10000 # Integer: Maximum number of iterations
min_num_iter: 200 # Integer: Minimum number of iterations
world_near_radius: 2.0 # Double (meters): Search radius (in world coordinates)
world_dist_dichotomy: 0.05 # Double (meters): Dichotomy distance (in world coordinates), the minimum allowable distance between nodes created in CreateNode
world_expand_dist: 5.0 # Double (meters): Maximum distance to expand tree when adding new nodes
world_goal_accessible_dist: 0.25 # Double (meters): Threshold for considering goal position to be accessible from current position
goal_frequency: 0.2 # Double (meters): Frequency of random node being set to goal node
lethal_cost: 128 # Integer (between 0 and 255 inclusive): Cost at which to consider a cell inaccessible (greater than or equal to this value), FREE_SPACE = 0, INSCRIBED_INFLATED_OBSTACLE = 253, LETHAL_OBSTACLE = 254, NO_INFORMATION = 255

BIN
docker/fast_rrt_ros/docs/Capture1.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 12 KiB

BIN
docker/fast_rrt_ros/docs/Capture2.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 14 KiB

BIN
docker/fast_rrt_ros/docs/Capture3.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 14 KiB

BIN
docker/fast_rrt_ros/docs/Capture4.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 13 KiB

BIN
docker/fast_rrt_ros/docs/Capture5.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 12 KiB

BIN
docker/fast_rrt_ros/docs/Capture6.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 13 KiB

BIN
docker/fast_rrt_ros/docs/Capture7.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 14 KiB

97
docker/fast_rrt_ros/docs/Fast_RRTdocumentation.md
View File

@@ -1,97 +0,0 @@
# Fast-RRT* Documentation
- [Fast-RRT* Documentation](#Fast-RRT-documentation)
- [`FindReachest()`](#findreachest)
- [Psuedo Code](#psuedo-code)
- [Written Explanation](#written-explanation)
- [Visual](#visual)
- [`CreateNode()`](#createnode)
- [Psuedo Code](#psuedo-code-1)
- [Written Explanation](#written-explanation-1)
- [Visual](#visual-1)
- [`Rewire()`](#rewire)
- [Written Explanation](#written-explanation-2)
- [Visual](#visual-2)
- [`SampleFree()`](#samplefree)
- [`Nearest()`](#nearest)
- [`Near()`](#near)
## `FindReachest()`
### Psuedo Code
<img src="findReachest.PNG" width="300">
### Written Explanation
Find the furthest possible node of `xNearest`'s parents from `xRand` that does not have an obstacle when a straight line is drawn between them.
The selected node `xReachest` must be chosen from `xNearest`'s ancestors. The starting node may not be selected as `xReachest`.
### Visual
<img src="findReachestSketch.PNG" width="500">
## `CreateNode()`
### Psuedo Code
<img src="createNode.PNG" width="300" >
### Written Explanation
Creates a node (`xCreate`) based on `xRand`, `xReachest`, and parent (`xReachest`) locations such that the path is drawn more near to the obstacle in few steps. Remember that these two nodes are guaranteed to have an obstacle between them.
The node is created between `xRand` and `xReachest` where the path between `xRand` and `xCreate` becomes free of all obstacles.
### Visual
<figure>
<img src="Capture1.PNG" width="380" >
<img src="Capture2.PNG" width="380" >
<figcaption> 1) xAllow and xForbid are set </figcaption>
</figure>
<figure>
<img src="Capture3.PNG" width="380" >
<figcaption> 2) xMid is established between xAllow and xForbid when the path is collision free to xRand</figcaption>
</figure>
<figure>
<img src="Capture4.PNG" width="380" >
<img src="Capture5.PNG" width="380" >
<figcaption> 3) xMid is the new xAllow and xRand is the new xForbid</figcaption>
</figure>
<figure>
<img src="Capture6.PNG" width="380" >
<figcaption> 4) a new xMid is established between xAllow and xForbid </figcaption>
</figure>
<figure>
<img src="Capture7.PNG" width="380" >
<figcaption> 4) a new xMid is established between xAllow and xForbid when collision free to parent(xReachest) </figcaption>
</figure>
## `Rewire()`
### Written Explanation
Changes connection relationships of tree vertices.
For any point in `xNear`, if replacing its parent node to be `xRand` reduces the cost of the path from `xStart` to `xNear`, then replace it.
### Visual
<img src="rewireSketch.PNG" width="150" >
## `SampleFree()`
Returns a sample `xRand` that is selected randomly from xFree.
## `Nearest()`
Nearest vector to `xRand` in terms of Euclidean distance.
## `Near()`
Return the set of vectors contained in specific radius around `xRand`.

BIN
docker/fast_rrt_ros/docs/createNode.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 104 KiB

BIN
docker/fast_rrt_ros/docs/fast_rrt.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 103 KiB

BIN
docker/fast_rrt_ros/docs/findReachest.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 37 KiB

BIN
docker/fast_rrt_ros/docs/findReachestSketch.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 21 KiB

BIN
docker/fast_rrt_ros/docs/move_base.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 62 KiB

BIN
docker/fast_rrt_ros/docs/rewireSketch.PNG
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 59 KiB

5
docker/fast_rrt_ros/fast_rrt_star_planner_plugin.xml
View File

@@ -1,5 +0,0 @@
<library path="lib/libfast_rrt_star_planner_lib">
<class name="fast_rrt_star_planner/Fast_RRTStarPlanner" type="fast_rrt_star_planner::Fast_RRTStarPlanner" base_class_type="nav_core::BaseGlobalPlanner">
<description>This is a global planner plugin that uses the Fast-RRT* algorithm.</description>
</class>
</library>

130
docker/fast_rrt_ros/include/fast_rrt_star_helper.h
View File

@@ -1,130 +0,0 @@
#ifndef FAST_RRT_STAR_HELPER_H
#define FAST_RRT_STAR_HELPER_H
#include "fast_rrt_star_node.h"
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <base_local_planner/footprint_helper.h>
#include <vector>
/**
* Create type aliases for map coordinates and cost
*/
using map_coords = unsigned int;
using map_cost = unsigned char;
namespace fast_rrt_star_planner
{
/**
* @brief Class with helper functions for Fast_RRTStarPlanner and Fast_RRTStarNode classes
*/
class Fast_RRTStarHelper
{
public:
/**
* @brief Find euclidean distance between two world coordinate points
*
* @param x0 First position x axis coordinate
* @param y0 First position y axis coordinate
* @param x1 Second position x axis coordinate
* @param y1 Second position y axis coordinate
* @return world_coords Euclidean distance between (x0,y0) and (x1,y1)
*/
static world_coords distance(world_coords x0, world_coords y0, world_coords x1, world_coords y1);
/**
* @brief Find euclidean distance between two nodes
*
* @param node0 First node
* @param node1 Second node
* @return world_coords Euclidean distance between node0 and node1
*/
static world_coords distance(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
/**
* @brief Find angle of line from node0 to node1
* relative to 0 radians being to the right
* (i.e., just like a unit circle in XY plane)
*
* @param node0 First node
* @param node1 Second node
* @return double Angle of line from node0 to node1 in radians
*/
static double angle(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
/**
* @brief Checks if line between two world coordinates is collision free
*
* @param costmap Map of environment
* @param lethal_cost Cost map value that corresponds to a region that is inaccessible
* @param wx0 X component of first point in world coordinates
* @param wy0 Y component of first point in world coordinates
* @param wx1 X component of second point in world coordinates
* @param wy1 Y coponent of second point in world coordinates
* @return true If line from (wx0,wy0) to (wx1,wy1) doesn't have collisions
* @return false If line from (wx0,wy0) to (wx1,wy1) does have collisions
*/
static bool isLineCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1);
/**
* @brief Checks if map coordinate point is collision free
*
* @param costmap Map of environment
* @param lethal_cost Cost map value that corresponds to a region that is inaccessible
* @param mx X component of point in map coordinates
* @param my Y component of point in map coordinates
* @return true If point has not collisions
* @return false If point does have collisions
*/
static bool isPointCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, map_coords mx, map_coords my);
/**
* @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
*
* @param costmap Map of environment
* @param start_node Starting node
* @param end_node Ending node
* @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
*/
static std::vector<base_local_planner::Position2DInt> Bresenham(costmap_2d::Costmap2D *costmap, Fast_RRTStarNode *start_node, Fast_RRTStarNode *end_node);
/**
* @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
*
* @param costmap Map of environment
* @param wx0 X component of first point in world coordinates
* @param wy0 Y component of first point in world coordinates
* @param wx1 X component of second point in world coordinates
* @param wy1 Y component of second point in world coordinates
* @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
*/
static std::vector<base_local_planner::Position2DInt> Bresenham(costmap_2d::Costmap2D *costmap, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1);
/**
* @brief Discretizes continuous line into grid cells and returns a vector of base_local_planner::Position2DInt.
*
* @param x0 X component of first point in map coordinates
* @param y0 Y component of first point in map coordinates
* @param x1 X component of second point in map coordinates
* @param y1 Y component of second point in map coordinates
* @return std::vector<base_local_planner::Position2DInt> Vector of costmap cells that ray traced line passes through
*/
static std::vector<base_local_planner::Position2DInt> Bresenham(map_coords x0, map_coords y0, map_coords x1, map_coords y1);
/**
* @brief Checks if path between two nodes is collision free when accounting for robot footprint and orientation
*
* @param costmap Map of environment
* @param costmap_ros ROS version of map of environment
* @param lethal_cost Cost map value that corresponds to a region that is inaccessible
* @param node0 First node
* @param node1 Second node
* @return true If path has no collisions with footprint of robot
* @return false If path does have collisions with footprint of robot
*/
static bool isFootprintPathCollisionFree(costmap_2d::Costmap2D *costmap, costmap_2d::Costmap2DROS *costmap_ros, map_cost lethal_cost, Fast_RRTStarNode *node0, Fast_RRTStarNode *node1);
};
}
#endif /* FAST_RRT_STAR_HELPER_H */

97
docker/fast_rrt_ros/include/fast_rrt_star_node.h
View File

@@ -1,97 +0,0 @@
#ifndef FAST_RRT_STAR_NODE_H
#define FAST_RRT_STAR_NODE_H
/**
* Create type alias for world coordinates
*/
using world_coords = double;
namespace fast_rrt_star_planner
{
/**
* @brief Fast_RRTStarNode class used to track nodes of path planning tree.
*/
class Fast_RRTStarNode
{
public:
/**
* @brief Public Variables
*/
world_coords x; ///< x coordinate of node
world_coords y; ///< y coordinate of node
world_coords path_cost; ///< Cumulative cost from node_start to current node
Fast_RRTStarNode *parent; ///< Pointer to parent of current node
/**
* @brief Construct a new Fast_RRTStarNode object with default values
*/
Fast_RRTStarNode();
/**
* @brief Construct a new Fast_RRTStarNode object with given position
*
* @param x_in X component of node in world coordinates
* @param y_in Y component of node in world coordinates
*/
Fast_RRTStarNode(world_coords x_in, world_coords y_in);
/**
* @brief Construct a new Fast_RRTStarNode object as a deep copy of pointer to input node
*
* @param node Pointer to input node to deep copy
*/
Fast_RRTStarNode(Fast_RRTStarNode *node);
/**
* @brief Construct a new Fast_RRTStarNode object as a deep copy of constant pointer to constant input node
*
* @param node Constant pointer to constant input node to deep copy
*/
Fast_RRTStarNode(const Fast_RRTStarNode *const node);
/**
* @brief "Connects" node to node_parent by updating parent pointer and path_cost
*
* @param node_parent Desired node being assigned as parent
*/
void updateParentAndCost(Fast_RRTStarNode *node_parent);
/**
* @brief "Connects" this node to node_parent by updating parent pointer and path_cost
* without computing distance between node and node_parent internally,
* just uses the provided cost (reason: saves re-computation)
*
* @param node_parent Desired node being assigned as parent
* @param path_cost Path cost (i.e. distance) between current node and node_parent
*/
void updateParentAndCost(Fast_RRTStarNode *node_parent, world_coords path_cost);
/**
* @brief Compute cost of connecting this node to node_connect (hypothetical parent)
*
* @param node_connect Node chosen to calculate cost to
* @return world_coords Cost of path between the two nodes
*/
world_coords getConnectionCost(Fast_RRTStarNode *node_connect);
/**
* @brief Overloaded equality operator to compare two Fast_RRTStarNodes
*
* @param node Node to compare to current node for equality
* @return true If properties of current and input node are the same
* @return false If properties of current and input node are not the same
*/
bool operator==(const Fast_RRTStarNode node);
/**
* @brief Overloaded inequality operator to compare two Fast_RRTStarNodes
*
* @param node Node to compare to current node for inequality
* @return true If properties of current and input node are not the same
* @return false If properties of current and input node are the same
*/
bool operator!=(const Fast_RRTStarNode node);
};
}
#endif /* FAST_RRT_STAR_NODE_H */

206
docker/fast_rrt_ros/include/fast_rrt_star_planner.h
View File

@@ -1,206 +0,0 @@
/**
* @brief This implementation largely follows what is described in ROS documentation:
* http://wiki.ros.org/navigation/Tutorials/Writing%20A%20Global%20Path%20Planner%20As%20Plugin%20in%20ROS
*/
#ifndef FAST_RRT_STAR_PLANNER_H
#define FAST_RRT_STAR_PLANNER_H
#include "fast_rrt_star_helper.h"
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_core/base_global_planner.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <angles/angles.h>
#include <tf/tf.h>
#include <base_local_planner/world_model.h>
#include <base_local_planner/costmap_model.h>
using std::string;
using pose_value_type = double;
namespace fast_rrt_star_planner
{
/**
* Main class for Fast-RRT* algorithm that extends move_base's BaseGlobalPlanner.
* Takes a starting location, goal location, and map of environment as input
* and generates a set of poses for local planner to follow to get from
* start location to goal location.
*
* NOTE on units and variable names:
* - map or m: Map coordinates (i.e., cell index)
* - world or w: World coordinates (i.e., meters)
*/
class Fast_RRTStarPlanner : public nav_core::BaseGlobalPlanner
{
private:
/**
* @brief Private Variables
*/
bool initialized_; ///< Flag indicating if planner is initialized
bool path_found_; ///< Flag indicating if planner found a path
world_coords best_path_cost_; ///< Track the shortest path distance found so far
Fast_RRTStarNode *node_best_path_end_; ///< Pointer to last node in best path
base_local_planner::CostmapModel *costmap_model_; ///< Used by planner to check for collisions
costmap_2d::Costmap2DROS *costmap_ros_; ///< ROS 2D Costmap
costmap_2d::Costmap2D *costmap_; ///< 2D Costmap
std::vector<Fast_RRTStarNode *> node_vec_; ///< Vector of planning nodes pointers
world_coords resolution_; ///< Costmap resolution (size of grid cell in world coordinates)
world_coords origin_x_; ///< x coordinate of costmap origin
world_coords origin_y_; ///< y coordinate of costmap origin
world_coords size_x_; ///< Size of costmap's x axis in world coordinates
world_coords size_y_; ///< Size of costmap's y axis in world coordinates
/**
* @brief Tunable Parameters
*/
int max_num_iter_; ///< Maximum number of iterations
int min_num_iter_; ///< Minimum number of iterations
world_coords world_near_radius_; ///< Search radius (in world coordinates)
world_coords world_dist_dichotomy_; ///< Dichotomy distance (in world coordinates), the minimum
///< allowable distance between nodes created in CreateNode
world_coords world_goal_accessible_dist_; ///< Threshold for considering goal position to be
///< accessible from current position
world_coords world_expand_dist_; ///< Maximum distance to expand tree when adding new nodes
double goal_frequency_; ///< Frequency of random node being set to goal node
map_cost lethal_cost_; ///< Cost at which to consider a cell inaccessible
/**
* @brief Randomly samples free space in costmap to generate a new node
*
* @param node_goal Fixed goal node
* @return Fast_RRTStarNode* Pointer to randomly sampled node
*/
Fast_RRTStarNode *sampleFree(const Fast_RRTStarNode *const node_goal);
/**
* @brief Find the index of node in existing tree that is closest to node_rand
*
* @param node_rand Randomly sampled node
* @return size_t Index of node in existing tree that is nearest to node_rand
*/
size_t nearest(Fast_RRTStarNode *node_rand);
/**
* @brief Find all nodes (as indices in near_vec_) in existing tree
* that are within world_near_radius_ of random node
*
* @param node_rand Randomly sampled node
* @return std::vector<size_t> Vector of node indices that are within world_near_radius_ of node_rand
*/
std::vector<size_t> near(Fast_RRTStarNode *node_rand);
/**
* @brief Find furthest possible away node from node_rand that is collision free.
* Only search through ancestors of node_nearest.
*
* @param node_nearest Nearest node in distance to randomly sampled node node_rand
* @param node_rand Randomly sampled node
* @return Fast_RRTStarNode* Pointer to node_reachest
*/
Fast_RRTStarNode *findReachest(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand);
/**
* @brief Create a new node to connect node_rand to node_nearest using
* world_dist_dichotomy_, node_reachest, node_rand, and costmap_.
*
* @param node_reachest Eldest ancestor node of node_rand that is collision free to node_rand
* @param node_rand Randomly sampled node
* @return Fast_RRTStarNode* Pointer to node_create
*/
Fast_RRTStarNode *createNode(Fast_RRTStarNode *node_reachest, Fast_RRTStarNode *node_rand);
/**
* @brief Check if latest node added to node_vec_ is within threshold
* distance of goal position
*
* @param node_goal Fixed goal node
* @return true if tree is close enough to node_goal
* @return false if tree is not close enough to node_goal
*/
bool initialPathFound(const Fast_RRTStarNode *const node_goal);
/**
* @brief Changes the connection relationships of tree vertices.
* If replacing the parent node of node_near by node_rand reduces path from start to node_near, then do so.
* Returns the re-ordered node vector
*
* @param node_rand Randomly sampled node
* @param near_nodes_list List of nodes within specified distance limit of node_rand
*/
void rewire(Fast_RRTStarNode *node_rand, const std::vector<size_t> &near_nodes_list);
/**
* @brief Construct the vector of stamped poses from linked list of
* Fast-RRT* node pointers
*
* @param start Time stamped pose of start position
* @param goal Time stamped pose of goal position
* @param plan List of stamped poses, starting with start node and ending with goal
* @param node_goal Fixed goal node
*/
void constructPlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan,
const Fast_RRTStarNode *const node_goal);
/**
* @brief Update node_rand so tree extends from node_nearest in direction of original
* node_rand until it reaches an occupied or unknown cell
*
* @param node_nearest Node nearest to sampled node
* @param node_rand Randomly sampled node
* @param node_goal Fixed goal node
*/
void steer(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand, const Fast_RRTStarNode *const node_goal);
public:
/**
* @brief Public Variables
*/
ros::Publisher plan_pub_;
std::string frame_id_;
/**
* @brief Construct a new Fast_RRTStarPlanner object.
* Initializes the planner attributes with the default values.
*/
Fast_RRTStarPlanner();
/**
* @brief Construct a new Fast_RRTStarPlanner object.
* Used to initialize the costmap, that is the map that will be used for planning (costmap_ros),
* and the name of the planner (name).
*
* @param name Name of the planner
* @param costmap_ros ROS version of costmap
*/
Fast_RRTStarPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
/**
* @brief Overridden class from interface nav_core::BaseGlobalPlanner
* An initialization function for the BaseGlobalPlanner, which initializes the costmap,
* that is the map that will be used for planning (costmap_ros), and the name of the planner (name).
*
* @param name Name of planner
* @param costmap_ros ROS version of costmap
*/
void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
/**
* @brief Overridden class from interface nav_core::BaseGlobalPlanner
*
* @param start Pose of starting position
* @param goal Pose of end position
* @param plan Vector of timestamped poses
* @return true if planner could find a path from start to goal
* @return false if planner could not find a path from start to goal
*/
bool makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan);
};
};
#endif /* FAST_RRT_STAR_PLANNER_H */

30
docker/fast_rrt_ros/package.xml
View File

@@ -1,30 +0,0 @@
<?xml version="1.0"?>
<package format="2">
<name>fast_rrt_ros</name>
<version>1.0.0</version>
<description>The fast_rrt_ros package</description>
<maintainer email="bcanaday@mitre.org">Bradley Canaday</maintainer>
<license>MIT</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>nav_core</build_depend>
<build_depend>base_local_planner</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>costmap_2d</exec_depend>
<exec_depend>nav_core</exec_depend>
<exec_depend>base_local_planner</exec_depend>
<export>
<nav_core plugin="${prefix}/fast_rrt_star_planner_plugin.xml" />
</export>
</package>

220
docker/fast_rrt_ros/src/fast_rrt_star_helper.cpp
View File

@@ -1,220 +0,0 @@
#include "fast_rrt_star_helper.h"
#include <ros/ros.h>
#include <cmath>
using namespace std;
namespace fast_rrt_star_planner
{
world_coords Fast_RRTStarHelper::distance(world_coords x0, world_coords y0, world_coords x1, world_coords y1)
{
// World (x,y) coordinate difference between candidate and random nodes
/**
* World (x,y) coordinate difference between candidate and random nodes
*/
world_coords diff_x = x0 - x1;
world_coords diff_y = y0 - y1;
/**
* Compute Euclidean distance between points
*/
world_coords dist = sqrt(pow(diff_x, 2) + pow(diff_y, 2));
return dist;
}
world_coords Fast_RRTStarHelper::distance(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
{
return distance(node0->x, node0->y, node1->x, node1->y);
}
double Fast_RRTStarHelper::angle(Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
{
world_coords wdx = node1->x - node0->x;
world_coords wdy = node1->y - node0->y;
double angle = atan2(wdy, wdx);
return angle;
}
bool Fast_RRTStarHelper::isLineCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1)
{
/**
* Bresenham Ray-Tracing
*/
vector<base_local_planner::Position2DInt> ray_traced_line = Bresenham(costmap, wx0, wy0, wx1, wy1);
for (size_t i = 0; i < ray_traced_line.size(); ++i)
{
/**
* Get cost
*/
map_coords mx = ray_traced_line[i].x;
map_coords my = ray_traced_line[i].y;
if (isPointCollisionFree(costmap, lethal_cost, mx, my) == false)
{
return false;
}
}
/**
* If no points had collisions, then the whole
* line has no collisions
*/
return true;
}
bool Fast_RRTStarHelper::isPointCollisionFree(costmap_2d::Costmap2D *costmap, map_cost lethal_cost, map_coords mx, map_coords my)
{
map_cost cell_cost = costmap->getCost(mx, my);
if (cell_cost >= lethal_cost)
{
return false;
}
return true;
}
vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(costmap_2d::Costmap2D *costmap, Fast_RRTStarNode *start_node, Fast_RRTStarNode *end_node)
{
if (start_node == nullptr)
{
ROS_ERROR("NULLPTR: start_node");
}
if (end_node == nullptr)
{
ROS_ERROR("NULLPTR: end_node");
}
world_coords start_wx = start_node->x;
world_coords start_wy = start_node->y;
world_coords end_wx = end_node->x;
world_coords end_wy = end_node->y;
return Bresenham(costmap, start_wx, start_wy, end_wx, end_wy);
}
vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(costmap_2d::Costmap2D *costmap, world_coords wx0, world_coords wy0, world_coords wx1, world_coords wy1)
{
map_coords mx0, mx1, my0, my1;
costmap->worldToMap(wx0, wy0, mx0, my0);
costmap->worldToMap(wx1, wy1, mx1, my1);
return Bresenham(mx0, my0, mx1, my1);
}
vector<base_local_planner::Position2DInt> Fast_RRTStarHelper::Bresenham(map_coords x0, map_coords y0, map_coords x1, map_coords y1)
{
vector<base_local_planner::Position2DInt> ray_traced_line;
base_local_planner::FootprintHelper fh;
fh.getLineCells(x0, x1, y0, y1, ray_traced_line);
return ray_traced_line;
}
bool Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_2d::Costmap2D *costmap, costmap_2d::Costmap2DROS *costmap_ros, map_cost lethal_cost, Fast_RRTStarNode *node0, Fast_RRTStarNode *node1)
{
/**
* Bresenham Ray-Tracing
*/
vector<base_local_planner::Position2DInt> ray_traced_line = Bresenham(costmap, node0, node1);
for (size_t i = 0; i < ray_traced_line.size(); ++i)
{
/**
* Check center point of robot
*/
map_coords mx = ray_traced_line[i].x;
map_coords my = ray_traced_line[i].y;
world_coords wx, wy;
costmap->mapToWorld(mx, my, wx, wy);
if (isPointCollisionFree(costmap, lethal_cost, mx, my) == false)
{
return false;
}
/**
* Get footprint and angle
*/
vector<geometry_msgs::Point> base_footprint = costmap_ros->getRobotFootprint();
double theta = angle(node0, node1);
/**
* Need to transform footprint points to a theoretical
* to check for collisions
*
* Construct rotation matrix for yaw angle theta
* R_z(theta) = [ cos(theta), -sin(theta), 0;
* sin(theta), cos(theta), 0;
* 0, 0, 1 ];
*/
vector<double> R_z_theta = {cos(theta), -sin(theta), 0, sin(theta), cos(theta), 0, 0, 0};
/**
* Construct translation matrix
* t(tx, ty, tz) = [ tx;
* ty;
* tz;
* 1; ]
*/
vector<double> t = {wx, wy, 0};
/**
* Transform base_footprint points with transformation matrix
* and populate oriented_footprint vector
*/
vector<geometry_msgs::Point> oriented_footprint;
for (size_t i = 0; i < base_footprint.size(); ++i)
{
geometry_msgs::Point old_pt = base_footprint[i];
geometry_msgs::Point new_pt;
/**
* Do matrix multiplication manually
*/
new_pt.x = old_pt.x * R_z_theta[0] + old_pt.y * R_z_theta[1] + old_pt.z * R_z_theta[2] + t[0];
new_pt.y = old_pt.x * R_z_theta[3] + old_pt.y * R_z_theta[4] + old_pt.z * R_z_theta[5] + t[1];
new_pt.z = old_pt.x * R_z_theta[6] + old_pt.y * R_z_theta[7] + old_pt.z * R_z_theta[8] + t[2];
oriented_footprint.push_back(new_pt);
}
/**
* Check lines connecting footprint vertices for collisions
*/
for (size_t i = 0; i < oriented_footprint.size(); ++i)
{
world_coords wx0 = oriented_footprint[i].x;
world_coords wy0 = oriented_footprint[i].y;
world_coords wx1, wy1;
if (i + 1 < oriented_footprint.size())
{
wx1 = oriented_footprint[i + 1].x;
wy1 = oriented_footprint[i + 1].y;
}
else
{
wx1 = oriented_footprint[0].x;
wy1 = oriented_footprint[0].y;
}
if (isLineCollisionFree(costmap, lethal_cost, wx0, wy0, wx1, wy1) == false)
{
return false;
}
}
}
return true;
}
}

55
docker/fast_rrt_ros/src/fast_rrt_star_node.cpp
View File

@@ -1,55 +0,0 @@
#include "fast_rrt_star_node.h"
#include "fast_rrt_star_helper.h"
using namespace std;
namespace fast_rrt_star_planner
{
Fast_RRTStarNode::Fast_RRTStarNode()
: x(0.0), y(0.0), parent(nullptr), path_cost(0.0) {}
Fast_RRTStarNode::Fast_RRTStarNode(world_coords x_in, world_coords y_in)
: x(x_in), y(y_in), parent(nullptr), path_cost(0.0) {}
Fast_RRTStarNode::Fast_RRTStarNode(Fast_RRTStarNode *node)
: x(node->x), y(node->y), parent(node->parent), path_cost(node->path_cost) {}
Fast_RRTStarNode::Fast_RRTStarNode(const Fast_RRTStarNode *const node)
: x(node->x), y(node->y), parent(node->parent), path_cost(node->path_cost) {}
void Fast_RRTStarNode::updateParentAndCost(Fast_RRTStarNode *node_parent)
{
this->parent = node_parent;
this->path_cost = node_parent->path_cost + this->getConnectionCost(node_parent);
return;
}
void Fast_RRTStarNode::updateParentAndCost(Fast_RRTStarNode *node_parent, world_coords path_cost)
{
this->parent = node_parent;
this->path_cost = path_cost;
return;
}
world_coords Fast_RRTStarNode::getConnectionCost(Fast_RRTStarNode *node_connect)
{
return Fast_RRTStarHelper::distance(node_connect, this);
}
bool Fast_RRTStarNode::operator==(const Fast_RRTStarNode node)
{
if (this->x == node.x && this->y == node.y)
{
return true;
}
return false;
}
bool Fast_RRTStarNode::operator!=(const Fast_RRTStarNode node)
{
return !(*this == node);
}
}

693
docker/fast_rrt_ros/src/fast_rrt_star_planner.cpp
View File

@@ -1,693 +0,0 @@
#include "fast_rrt_star_planner.h"
#include <pluginlib/class_list_macros.h>
#include <stdlib.h>
#include <limits>
#include <random>
PLUGINLIB_EXPORT_CLASS(fast_rrt_star_planner::Fast_RRTStarPlanner, nav_core::BaseGlobalPlanner)
using namespace std;
namespace fast_rrt_star_planner
{
Fast_RRTStarPlanner::Fast_RRTStarPlanner()
: costmap_ros_(nullptr), initialized_(false) {}
Fast_RRTStarPlanner::Fast_RRTStarPlanner(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
: costmap_ros_(nullptr), initialized_(false)
{
initialize(name, costmap_ros);
}
void Fast_RRTStarPlanner::initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros)
{
if (initialized_ == false)
{
costmap_ros_ = costmap_ros; ///< initialize the costmap_ros_ attribute to the parameter
costmap_ = costmap_ros_->getCostmap(); ///< get the costmap_ from costmap_ros_
costmap_model_ = new base_local_planner::CostmapModel(*costmap_);
resolution_ = costmap_->getResolution();
origin_x_ = costmap_->getOriginX();
origin_y_ = costmap_->getOriginY();
size_x_ = costmap_->getSizeInMetersX();
size_y_ = costmap_->getSizeInMetersY();
int lethal_cost_int;
/**
* Initialize planner parameters from parameter server
* Reference: http://wiki.ros.org/roscpp/Overview/Parameter%20Server
*/
ros::NodeHandle nh("~/" + name);
nh.param("max_num_iter", max_num_iter_, 500);
nh.param("min_num_iter", min_num_iter_, 40);
nh.param("world_near_radius", world_near_radius_, 2.0);
nh.param("world_dist_dichotomy", world_dist_dichotomy_, 0.1);
nh.param("world_expand_dist", world_expand_dist_, 2.5);
nh.param("world_goal_accessible_dist", world_goal_accessible_dist_, 0.5);
nh.param("goal_frequency", goal_frequency_, 0.2);
nh.param("lethal_cost", lethal_cost_int, 253);
lethal_cost_ = static_cast<map_cost>(lethal_cost_int);
frame_id_ = costmap_ros->getGlobalFrameID();
plan_pub_ = nh.advertise<nav_msgs::Path>("plan", 1);
initialized_ = true;
}
else
{
ROS_WARN("This planner has already been initialized... doing nothing");
}
}
bool Fast_RRTStarPlanner::makePlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
std::vector<geometry_msgs::PoseStamped> &plan)
{
if (initialized_ == false)
{
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
/**
* Get updated costmap
*/
plan.clear();
costmap_ = costmap_ros_->getCostmap();
if (start.header.frame_id != frame_id_)
{
ROS_ERROR("This planner as configured will only accept start poses in the %s frame, but a start was sent in the %s frame.",
frame_id_.c_str(), start.header.frame_id.c_str());
}
if (goal.header.frame_id != frame_id_)
{
ROS_ERROR("This planner as configured will only accept goal poses in the %s frame, but a goal was sent in the %s frame.",
frame_id_.c_str(), goal.header.frame_id.c_str());
}
ROS_INFO("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
/**
* Reset global plan variables
*
*/
path_found_ = false;
best_path_cost_ = numeric_limits<double>::max();
node_best_path_end_ = nullptr;
/**
* Create nodes for start and goal poses
* These are constant pointers to a const, so neither the pointer nor the values it points to can change
*/
const Fast_RRTStarNode *const node_start = new Fast_RRTStarNode(start.pose.position.x, start.pose.position.y);
const Fast_RRTStarNode *const node_goal = new Fast_RRTStarNode(goal.pose.position.x, goal.pose.position.y);
/**
* Initialize node vector with start node
*/
Fast_RRTStarNode *node_start_copy = new Fast_RRTStarNode(node_start);
node_vec_.push_back(node_start_copy);
/**
* For loop over maximum number of iterations
*/
for (int i = 0; i < max_num_iter_; ++i)
{
/**
* Find a random unoccupied node
*/
Fast_RRTStarNode *node_rand = sampleFree(node_goal); ///< node's parent is not defined yet
/**
* nearest() to find index of node in existing tree
* that is closest to random position from
* SampleFree()
*/
size_t node_nearest_ind = nearest(node_rand);
Fast_RRTStarNode *node_nearest = node_vec_[node_nearest_ind];
/**
* Expand tree from node_nearest in direction of node_rand
* until distance limit is reached
*/
steer(node_nearest, node_rand, node_goal);
/**
* Check if straight line path has collision with obstacles
*/
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_nearest, node_rand) == true)
{
/**
* near() to find all nodes in existing tree
* that are within R_near of random node
*/
vector<size_t> near_nodes_list = near(node_rand); ///< Used by rewire()
/**
* findReachest() to find reachest node based
* on nearest node and random node and map
*/
Fast_RRTStarNode *node_reachest = findReachest(node_nearest, node_rand);
/**
* createNode() to create new node to connect
* random node to nearest node using D_dichotomy
* and reachest node and random node and map
*/
Fast_RRTStarNode *node_create = createNode(node_reachest, node_rand);
/**
* Check if node was able to be created
*/
if (node_create != nullptr)
{
/**
* If node was created, add created node and random node to tree
* ! NOTE: Make sure node_rand is appended AFTER node_create
* in node_vec_ so it is the last element
*/
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_create, node_reachest->parent) == false)
{
ROS_WARN("Missed collision check b/w node_create and node_reachest->parent");
}
node_create->updateParentAndCost(node_reachest->parent);
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_create) == false)
{
ROS_WARN("Missed collision check b/w node_rand and node_create");
}
node_rand->updateParentAndCost(node_create);
node_vec_.push_back(node_create);
node_vec_.push_back(node_rand);
}
else
{
/**
* If node was not created, add only random node to tree
*/
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_reachest) == false)
{
ROS_WARN("Missed collision check b/w node_rand and node_reachest");
}
node_rand->updateParentAndCost(node_reachest);
node_vec_.push_back(node_rand);
}
/**
* Check if initial path was found
* Compute distance between goal location and node_vec_.back() location
*/
if (initialPathFound(node_goal) == true)
{
path_found_ = true;
double current_path_cost = node_vec_.back()->path_cost;
if (current_path_cost < best_path_cost_)
{
best_path_cost_ = current_path_cost;
node_best_path_end_ = node_vec_.back();
}
if (i > min_num_iter_)
{
/**
* Exit for loop early if a valid plan was found
* after the minimum number of iterations
*/
break;
}
}
/**
* rewire() to improve path cost
*/
rewire(node_rand, near_nodes_list);
}
else
{
delete node_rand;
}
}
if (path_found_ == true)
{
ROS_INFO("PATH FOUND");
constructPlan(start, goal, plan, node_goal);
}
else
{
ROS_INFO("NO PATH FOUND");
}
/**
* Cleanup pointers when done
*/
delete node_start;
delete node_goal;
for (size_t i = 0; i < node_vec_.size(); ++i)
{
delete node_vec_[i];
}
node_vec_.clear();
return path_found_;
}
Fast_RRTStarNode *Fast_RRTStarPlanner::sampleFree(const Fast_RRTStarNode *const node_goal)
{
/**
* Random sampling setup
*/
std::random_device rd; ///< Will be used to obtain a seed for the random number engine
std::mt19937 gen(rd()); ///< Standard mersenne_twister_engine seeded with rd()
/**
* Sample between 0 and 1
*/
uniform_real_distribution<double> rand_is_goal_dist(0, 1);
double rand_is_goal_val = rand_is_goal_dist(gen);
/**
* If sample is between 0 and goal_frequency, set node_rand to node_goal
*/
if (rand_is_goal_val < goal_frequency_)
{
Fast_RRTStarNode *node_rand = new Fast_RRTStarNode(node_goal);
return node_rand;
}
/**
* Otherwise do stuff below
*/
Fast_RRTStarNode *node_rand = new Fast_RRTStarNode();
/**
* Get lower and upper bounds of costmap in world coordinates
*/
world_coords rand_wx_lb = origin_x_;
world_coords rand_wx_ub = origin_x_ + size_x_;
world_coords rand_wy_lb = origin_y_;
world_coords rand_wy_ub = origin_y_ + size_y_;
/**
* Create uniform distributions between lower and upper bound for x and y
*/
uniform_real_distribution<world_coords> rand_wx_dist(rand_wx_lb, rand_wx_ub);
uniform_real_distribution<world_coords> rand_wy_dist(rand_wy_lb, rand_wy_ub);
/**
* Sample from uniform distributions with random engine
*/
world_coords rand_wx = rand_wx_dist(gen);
world_coords rand_wy = rand_wy_dist(gen);
/**
* Update node_rand with randomly generated position
*/
node_rand->x = rand_wx;
node_rand->y = rand_wy;
return node_rand;
}
size_t Fast_RRTStarPlanner::nearest(Fast_RRTStarNode *node_rand)
{
/**
* Set initial index and world coordinate distance of nearest node
*/
size_t nearest_node_ind = 0;
world_coords nearest_node_cost = std::numeric_limits<world_coords>::max();
/**
* Search through node_vec_ to find closest node (based on connection cost)
*/
for (size_t i = 0; i < node_vec_.size(); ++i)
{
world_coords candidate_cost = node_rand->getConnectionCost(node_vec_[i]);
/**
* Update nearest node index and nearest node distance
* if candidate node is closer than previous closest node
*/
if (candidate_cost < nearest_node_cost)
{
nearest_node_cost = candidate_cost;
nearest_node_ind = i;
}
}
return nearest_node_ind;
}
vector<size_t> Fast_RRTStarPlanner::near(Fast_RRTStarNode *node_rand)
{
/**
* Create vector to hold indexes of near nodes
*/
vector<size_t> near_node_idxs;
/**
* Loop through node_vec_
*/
for (size_t i = 0; i < node_vec_.size(); ++i)
{
/**
* Check if distance between node_rand and node in node_vec_
* is less than the near radius
*/
if (Fast_RRTStarHelper::distance(node_rand, node_vec_[i]) < world_near_radius_)
{
/**
* Add near node index to near_node_idxs vector
*/
near_node_idxs.push_back(i);
}
}
return near_node_idxs;
}
Fast_RRTStarNode *Fast_RRTStarPlanner::findReachest(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand)
{
/**
* Set new node to node_nearest
*/
Fast_RRTStarNode *node_reachest = new Fast_RRTStarNode(node_nearest);
/**
* While not back at start node, search through node_nearest parents
* to find the furthest collision free node from node_rand
* grab start node, first pointer in node_vec
*/
Fast_RRTStarNode *node_start = node_vec_.front();
while (*node_start != *node_reachest)
{
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_reachest->parent) == true)
{
/**
* Create temporary pointer to parent of node_reachest
*/
Fast_RRTStarNode *node_reachest_parent = node_reachest->parent;
/**
* Delete old node_reachest
*/
delete node_reachest;
/**
* Create new node_reachest as copy of old node_reachest parent
*/
node_reachest = new Fast_RRTStarNode(node_reachest_parent);
}
else
{
break;
}
}
return node_reachest;
}
Fast_RRTStarNode *Fast_RRTStarPlanner::createNode(Fast_RRTStarNode *node_reachest, Fast_RRTStarNode *node_rand)
{
Fast_RRTStarNode *node_allow = new Fast_RRTStarNode(node_reachest);
Fast_RRTStarNode *node_start = node_vec_.front();
Fast_RRTStarNode *node_mid = new Fast_RRTStarNode();
Fast_RRTStarNode *node_forbid = nullptr;
if (*node_start != *node_reachest)
{
node_forbid = new Fast_RRTStarNode(node_reachest->parent);
/**
* while allow/forbid nodes distance > dichotomy distance
*/
while (Fast_RRTStarHelper::distance(node_allow, node_forbid) > world_dist_dichotomy_)
{
node_mid->x = (node_allow->x + node_forbid->x) / 2.0;
node_mid->y = (node_allow->y + node_forbid->y) / 2.0;
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_rand, node_mid) == true)
{
node_allow->x = node_mid->x;
node_allow->y = node_mid->y;
}
else
{
node_forbid->x = node_mid->x;
node_forbid->y = node_mid->y;
}
}
node_forbid->x = node_rand->x;
node_forbid->y = node_rand->y;
while (Fast_RRTStarHelper::distance(node_allow, node_forbid) > world_dist_dichotomy_)
{
node_mid->x = (node_allow->x + node_forbid->x) / 2.0;
node_mid->y = (node_allow->y + node_forbid->y) / 2.0;
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_mid, node_reachest->parent) == true)
{
node_allow->x = node_mid->x;
node_allow->y = node_mid->y;
}
else
{
node_forbid->x = node_mid->x;
node_forbid->y = node_mid->y;
}
}
}
Fast_RRTStarNode *node_create = new Fast_RRTStarNode();
if (*node_allow != *node_reachest)
{
node_create->x = node_allow->x;
node_create->y = node_allow->y;
}
else
{
node_create = nullptr;
}
delete node_allow;
delete node_mid;
delete node_forbid;
return node_create;
}
void Fast_RRTStarPlanner::rewire(Fast_RRTStarNode *node_rand, const vector<size_t> &near_nodes_list)
{
/**
* for each node in near
*/
for (size_t i = 0; i < near_nodes_list.size(); ++i)
{
size_t node_near_index_i = near_nodes_list[i];
Fast_RRTStarNode *node_near_i = node_vec_[node_near_index_i];
world_coords rand_rewire_cost = node_rand->path_cost + node_near_i->getConnectionCost(node_rand);
if (node_near_i->path_cost > rand_rewire_cost)
{
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_near_i, node_rand) == true)
{
node_near_i->updateParentAndCost(node_rand, rand_rewire_cost);
}
}
}
}
bool Fast_RRTStarPlanner::initialPathFound(const Fast_RRTStarNode *const node_goal)
{
Fast_RRTStarNode *node_goal_copy = new Fast_RRTStarNode(node_goal);
Fast_RRTStarNode *node_last = node_vec_.back();
world_coords dist_to_goal = Fast_RRTStarHelper::distance(node_last, node_goal_copy);
bool goal_dist_threshold_met = dist_to_goal < world_goal_accessible_dist_;
bool last_to_goal_collision_free = Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_last, node_goal_copy);
bool path_collision_free = true;
Fast_RRTStarNode *node_n = node_last;
/**
* Verify that path is still collision free even if map has changed
*/
while (node_n->parent != nullptr)
{
if (!Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_n, node_n->parent) == true)
{
path_collision_free = false;
break;
}
node_n = node_n->parent;
}
delete node_goal_copy;
return goal_dist_threshold_met && last_to_goal_collision_free && path_collision_free;
}
void Fast_RRTStarPlanner::constructPlan(const geometry_msgs::PoseStamped &start,
const geometry_msgs::PoseStamped &goal,
vector<geometry_msgs::PoseStamped> &plan,
const Fast_RRTStarNode *const node_goal)
{
/**
* Empty plan vector
*/
plan.clear();
ros::Time plan_time = ros::Time::now();
/**
* Create a node for the goal position
*/
Fast_RRTStarNode *node_goal_copy = new Fast_RRTStarNode(node_goal);
if (Fast_RRTStarHelper::isFootprintPathCollisionFree(costmap_, costmap_ros_, lethal_cost_, node_goal_copy, node_best_path_end_) == false)
{
ROS_WARN("Missed collision check b/w node_goal_copy and node_best_path_end_");
}
node_goal_copy->updateParentAndCost(node_best_path_end_);
node_vec_.push_back(node_goal_copy);
/**
* Initialize node_n to goal node
*/
Fast_RRTStarNode *node_n = node_vec_.back();
bool isGoalNode = true;
pose_value_type zVal = 0.0; ///< Z-axis is ignored
/**
* Go through parents of node until hitting start_node
*/
while (node_n->parent != nullptr)
{
/**
* Initialize new stamped pose
*/
geometry_msgs::PoseStamped parent_pose_n;
/**
* Plan headers
*/
parent_pose_n.header.stamp = plan_time;
parent_pose_n.header.frame_id = frame_id_;
/**
* Set position
*/
parent_pose_n.pose.position.x = node_n->parent->x;
parent_pose_n.pose.position.y = node_n->parent->y;
parent_pose_n.pose.position.z = zVal;
/**
* Set orientation
* ! NOTE: Assuming FLU (x: Front, y: Left, z: Up) coordinate frame
* This means 0 yaw points front
* Positive yaw is CCW and negative yaw is CW.
*/
world_coords start_x = node_n->parent->x;
world_coords start_y = node_n->parent->y;
world_coords end_x = node_n->x;
world_coords end_y = node_n->y;
world_coords dx = end_x - start_x;
world_coords dy = end_y - start_y;
pose_value_type yaw_in_rad = -atan2(dx, dy);
tf::Quaternion parent_pose_n_quat = tf::createQuaternionFromYaw(yaw_in_rad);
parent_pose_n.pose.orientation.x = parent_pose_n_quat.x();
parent_pose_n.pose.orientation.y = parent_pose_n_quat.y();
parent_pose_n.pose.orientation.z = parent_pose_n_quat.z();
parent_pose_n.pose.orientation.w = parent_pose_n_quat.w();
/**
* Goal node re-uses orientation of parent, but updates position
* NOTE: Make sure to push back goal pose first so reverse
* ordering of poses is correct
*/
if (isGoalNode == true)
{
geometry_msgs::PoseStamped pose_n = parent_pose_n;
/**
* Set position
*/
pose_n.pose.position.x = node_n->x;
pose_n.pose.position.y = node_n->y;
/**
* Set orientation
*/
pose_n.pose.orientation = goal.pose.orientation;
/**
* Add pose of goal node to plan
*/
plan.push_back(pose_n);
isGoalNode = false;
}
/**
* Add pose of parent node to plan
*/
plan.push_back(parent_pose_n);
/**
* Update node_n to its parent
*/
node_n = node_n->parent;
}
/**
* Reverse order of plan so pose associated with start_node is first
* and pose associated with goal_node is last
*/
reverse(plan.begin(), plan.end());
return;
}
void Fast_RRTStarPlanner::steer(Fast_RRTStarNode *node_nearest, Fast_RRTStarNode *node_rand, const Fast_RRTStarNode *const node_goal)
{
/**
* Calculate dist of vector from node_nearest to node_rand
*/
world_coords dist = Fast_RRTStarHelper::distance(node_nearest, node_rand);
/**
* If node_rand is within expand dist, keep node_rand as is
*/
if (dist < world_expand_dist_)
{
node_rand->updateParentAndCost(node_nearest);
return;
}
double angle = Fast_RRTStarHelper::angle(node_nearest, node_rand);
/**
* Re-calculate x/y components of vector based on new distance
*/
node_rand->x = node_nearest->x + world_expand_dist_ * cos(angle);
node_rand->y = node_nearest->y + world_expand_dist_ * sin(angle);
return;
}
};

3
docker/scripts/1_gazebo.bash
View File

@@ -1,3 +0,0 @@
#!/bin/bash
roslaunch husky_gazebo husky_playpen.launch

3
docker/scripts/2_rviz.bash
View File

@@ -1,3 +0,0 @@
#!/bin/bash
roslaunch husky_viz view_robot.launch

7
docker/scripts/3_default.bash
View File

@@ -1,7 +0,0 @@
#!/bin/bash
source /opt/ros/$ROS_DISTRO/setup.bash && \
rm -rf build/ devel/ && \
catkin_make -DCMAKE_BUILD_TYPE=Release && \
source devel/setup.bash && \
roslaunch husky_navigation move_base_mapless_demo.launch

7
docker/scripts/3_fast_rrt_star.bash
View File

@@ -1,7 +0,0 @@
#!/bin/bash
source /opt/ros/$ROS_DISTRO/setup.bash && \
rm -rf build/ devel/ && \
catkin_make -DCMAKE_BUILD_TYPE=Release && \
source devel/setup.bash && \
roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo.launch

7
docker/scripts/3_fast_rrt_star_debug.bash
View File

@@ -1,7 +0,0 @@
#!/bin/bash
source /opt/ros/$ROS_DISTRO/setup.bash && \
rm -rf build/ devel/ && \
catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo && \
source devel/setup.bash && \
roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo.launch

7
docker/scripts/3_fast_rrt_star_debug_valgrind.bash
View File

@@ -1,7 +0,0 @@
#!/bin/bash
source /opt/ros/$ROS_DISTRO/setup.bash && \
rm -rf build/ devel/ && \
catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo && \
source devel/setup.bash && \
roslaunch husky_navigation move_base_fast_rrt_star_mapless_demo_debug.launch