FMTstar-planner
This repository provides a path-planning ROS Action Server which uses Heuristic FMT*(Forward Marching Tree) planner for 2D ROS occupancy grids in ROS to generate asymptotically optimal paths at a rate much faster than state of the art algorithms like RRT* and PRM*.
Steps to run it:
- Clone the Repository
cd catkin_ws/src
git clone https://github.com/YashTrikannad/FMTstar-planner.git
catkin_make install
catkin_make
- Run the F110 simulator (Make sure you have it cloned on your local machine (Follow the instructions in this repository)- mLab)
cd catkin_ws
source devel/setup.bash
roslaunch racecar_simulator simulator.launch
- Open a new terminal. Run the Action Server Node
cd catkin_ws
source devel/setup.bash
roslaunch fmt_star fmt_star_server.launch
- Open a new terminal. Run the Client Node to query the planner
cd catkin_ws
source devel/setup.bash
rosrun fmt_star fmt_star_test_client
Configuration Parameters can be changed in the config/config.yaml file to get the desired behavior and efficiency
How to call the planner and recieve plans
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <fmt_star/planAction.h>
Include the required headers
int main (int argc, char **argv)
{
ros::init(argc, argv, "fmt_star_test_client");
actionlib::SimpleActionClient<fmt_star::planAction> ac("fmt_star_server", true);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending goal.");
Initialize your client node where you want to recieve the path and add add this action service that the client node will be connecting to for recieveing the path messages.
const auto start = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("gt_pose_1",ros::Duration(2));
ROS_INFO("Send a 2d Nav Goal");
const auto end = ros::topic::waitForMessage<geometry_msgs::PoseStamped>("move_base_simple/goal",ros::Duration(20));
Get your start and goal messages which are geometry_msgs::PoseStamped
fmt_star::planGoal goal;
if(!start)
{
ROS_ERROR("Unable to Plan. Start not recieved");
}
else if(!end)
{
ROS_ERROR("Unable to Plan. Goal not recieved");
}
else
{
goal.start_position = *start;
goal.end_position = *end;
goal.update_map = false;
goal.update_samples = true;
}
Assign your start and goal to the service request
ac.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();
ROS_INFO("Action finished. Plan Recieved: %s",state.toString().c_str());
}
else
{
ROS_INFO("Action did not finish before the time out.");
}
return 0;
}
Send the Goal to the action server and wait for the result.
You can find the Test Client Example here
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent