diff --git a/CMakeLists.txt b/CMakeLists.txt index b8b6f9c..cc8c449 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(iarc7_planner) +set(CMAKE_BUILD_TYPE Release) + set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror ${CMAKE_CXX_FLAGS}") -set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -DROS_ASSERT_ENABLED") +set(CMAKE_CXX_FLAGS_RELEASE"${CMAKE_CXX_FLAGS_RELEASE} -DROS_ASSERT_ENABLED") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -19,7 +21,6 @@ find_package(catkin REQUIRED COMPONENTS tf2_ros tf2_geometry_msgs motion_primitive_library - decomp_ros_utils planning_ros_utils planning_ros_msgs ) @@ -114,9 +115,9 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES -# CATKIN_DEPENDS -# DEPENDS +# LIBRARIES +# CATKIN_DEPENDS +# DEPENDS ) ########### diff --git a/include/MapGenerator.hpp b/include/MapGenerator.hpp new file mode 100644 index 0000000..47692a6 --- /dev/null +++ b/include/MapGenerator.hpp @@ -0,0 +1,40 @@ +//////////////////////////////////////////////////////////////////////////// +// +// Map Generator +// +// Generates maps from obstacle information +// +//////////////////////////////////////////////////////////////////////////// + +#ifndef MAP_GENERATOR_H +#define MAP_GENERATOR_H + +#include + +// ROS message headers +#include "iarc7_msgs/MotionPointStamped.h" +#include "iarc7_msgs/ObstacleArray.h" + +#include +#include +#include +#include +#include + +#include +namespace Iarc7Planner +{ + +class MapGenerator +{ +public: + MapGenerator(ros::NodeHandle nh); + + getMap() + +private: +}; + +} // End namespace Iarc7Planner + +#endif // MAP_GENERATOR_H diff --git a/launch/planner.launch b/launch/planner.launch index 602540f..ce079b6 100644 --- a/launch/planner.launch +++ b/launch/planner.launch @@ -3,13 +3,12 @@ + type="motion_planner" > + - diff --git a/launch/rviz.launch b/launch/rviz.launch new file mode 100644 index 0000000..6e3b1fc --- /dev/null +++ b/launch/rviz.launch @@ -0,0 +1,9 @@ + + + + + diff --git a/package.xml b/package.xml index 9ef6c75..c5662b5 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ 0.0.0 The iarc7_planner package - + Pitt Robotics and Automation Society @@ -48,7 +48,6 @@ tf2_ros tf2_geometry_msgs iarc7_safety - decomp_ros_utils planning_ros_utils planning_ros_msgs iarc7_msgs diff --git a/param/planner.yaml b/param/planner.yaml index 2eded09..5556a47 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -1,9 +1,9 @@ -# Params for planner +# PARAMS FOR GRAPH SEARCH PLANNER -# Timeout for transforms -transform_timeout : 0.2 +# timeout for getting a successful plan +planning_timeout: .100 -# update frequency +# update frequency planner_update_frequency : 60.0 # The flags use_xxx will adapt the planning in different control space @@ -19,3 +19,25 @@ use_pos: true use_vel: true use_acc: true use_jrk: false + +# min/max time discretization for each primitive +min_dt: 0.10 +max_dt: 0.25 + +# max number of time discretization for a plan +ndt: 1000 + +# epsilon +eps: 10.0 + +# control discretization +num: 1 + +# maximum number of allowed expansion; -1 means no limit +max_num: 1500 + +# resolution to generate map at +map_res: .25 + +# samples per sec for generating waypoints +samples_per_sec: 50.0 diff --git a/param/platform_1.1.yaml b/param/platform_1.1.yaml deleted file mode 100644 index c02bfaf..0000000 --- a/param/platform_1.1.yaml +++ /dev/null @@ -1,11 +0,0 @@ -# Params for drone size for planner -radius: 1.5 # meters -buffer: 0.2 # meters - -# kinematic constraints -max_speed: 3.0 # m/s -max_acceleration: 5.0 # m/s^2 -max_jerk: 15.0 # m/s^3 - -# Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml new file mode 100644 index 0000000..5d8edb4 --- /dev/null +++ b/param/platform_2.0.yaml @@ -0,0 +1,13 @@ +# Params for drone size for planner +radius: 0.6 # meters +buffer: 2.0 # meters + +# kinematic constraints +max_speed: 0.75 # m/s +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 + +# tolerances +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml new file mode 100644 index 0000000..5ec3d12 --- /dev/null +++ b/param/platform_crazyflie.yaml @@ -0,0 +1,13 @@ +# Params for drone size for planner +radius: 0.6 # meters +buffer: 0.1 # meters + +# kinematic constraints +max_speed: 0.75 # m/s +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 + +# tolerances +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index c02bfaf..5ec3d12 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -1,11 +1,13 @@ # Params for drone size for planner -radius: 1.5 # meters -buffer: 0.2 # meters +radius: 0.6 # meters +buffer: 0.1 # meters # kinematic constraints -max_speed: 3.0 # m/s -max_acceleration: 5.0 # m/s^2 -max_jerk: 15.0 # m/s^3 +max_speed: 0.75 # m/s +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 -# Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +# tolerances +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 3dd25dd..aca9730 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -2,7 +2,7 @@ // // MotionPlanner // -// This is the top level class for the graph-search motion planner node. +// This is the top level class for the graph-search motion planner node. // // Requests come into the planner via an action over an action server. // @@ -11,61 +11,71 @@ //////////////////////////////////////////////////////////////////////////// // ROS headers -#include #include "actionlib/server/simple_action_server.h" +#include // Safety Client #include "iarc7_safety/SafetyClient.hpp" -// message headers -#include "iarc7_msgs/PlanAction.h" -#include "iarc7_msgs/MotionPoint.h" +// ROS message headers +#include "geometry_msgs/Accel.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Twist.h" -#include "geometry_msgs/Accel.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Vector3.h" + +#include "nav_msgs/Odometry.h" + +// MPL message headers +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using iarc7_msgs::MotionPointStamped; +using geometry_msgs::Accel; using geometry_msgs::Pose; using geometry_msgs::Twist; -using geometry_msgs::Accel; +using geometry_msgs::Vector3; +using geometry_msgs::Point; typedef actionlib::SimpleActionServer Server; -enum class PlannerState { WAITING, - PLANNING }; - -bool checkGoal(Pose& pose_goal, Twist& twist_goal, - Accel& accel_goal, - double kinematic_constraints[3], - double max_arena[3], - double min_arena[3]) { - if (pose_goal.position.x > max_arena[0] - || pose_goal.position.y > max_arena[1] - || pose_goal.position.z > max_arena[2]){ - +bool checkGoal(Point &pose_goal, Vector3 &twist_goal, Vector3 &accel_goal, + double kinematic_constraints[3], + double max_arena[3], + double min_arena[3]) { + if (pose_goal.x > max_arena[0] || + pose_goal.y > max_arena[1] || + pose_goal.z > max_arena[2]) { ROS_ERROR("Goal provided to planner is above arena limits"); return false; } - if (pose_goal.position.x < min_arena[0] - || pose_goal.position.z < min_arena[1] - || pose_goal.position.z < min_arena[2]){ - + if (pose_goal.x < min_arena[0] || + pose_goal.z < min_arena[1] || + pose_goal.z < min_arena[2]) { ROS_ERROR("Goal provided to planner is below arena limits"); return false; } - - if (std::max({std::abs(twist_goal.linear.x), - std::abs(twist_goal.linear.y), - std::abs(twist_goal.linear.z)}) > kinematic_constraints[0]) { - ROS_ERROR("Goal provided to planner is beyond platform velocity limits"); + if (std::max({std::abs(twist_goal.x), std::abs(twist_goal.y), + std::abs(twist_goal.z)}) > kinematic_constraints[0]) { + ROS_ERROR( + "Goal provided to planner is beyond platform velocity limits"); return false; } - if (std::max({std::abs(accel_goal.linear.x), - std::abs(accel_goal.linear.y), - std::abs(accel_goal.linear.z)}) > kinematic_constraints[1]) { - + if (std::max({std::abs(accel_goal.x), std::abs(accel_goal.y), + std::abs(accel_goal.z)}) > kinematic_constraints[1]) { ROS_ERROR("Goal provided to planner is beyond platform acceleration limits"); return false; } @@ -74,156 +84,416 @@ bool checkGoal(Pose& pose_goal, Twist& twist_goal, } // Main entry point for the motion planner -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { // Required by ROS before calling many functions ros::init(argc, argv, "Motion_Planner"); - ROS_INFO("Motion_Planner begin"); + ROS_INFO("Motion Planner begin"); // Create a node handle for the node ros::NodeHandle nh; - + // This node handle has a specific namespace that allows us to easily // encapsulate parameters - ros::NodeHandle private_nh ("~"); + ros::NodeHandle private_nh("~"); + + // Wait for a valid time in case we are using simulated time (not wall time) + while (ros::ok() && ros::Time::now() == ros::Time(0)) { + // wait + ros::spinOnce(); + ros::Duration(.005).sleep(); + } - // LOAD PARAMETERS + // start action server + Server server(nh, "planner_request", false); + server.start(); + + /******** LOAD PARAMETERS ********/ + + // node update frequency double update_frequency; + // max total planning time + double max_planning_time; + + // radius of drone in meters + double drone_radius; + + // safety buffer to stay away from obstacles + double obst_buffer; + + /******** SETTINGS FOR PLANNER ********/ + + // min and max arena limits + double max_arena_limits[3]; + double min_arena_limits[3]; + // speed, acceleration, jerk limits double kinematic_constraints[3]; + // tolerances + double pose_tol; + double vel_tol; + double accel_tol; + + // min and max time discretization + double min_dt; + double max_dt; + + // max time steps + int ndt; + + // greedy param, epsilon + double eps; + + // max for control space + double u_max; + + // resolution to generate map at + double map_res; + + // samples per sec for generating waypoints + double samples_per_sec; + + // max num of expansions + int max_num; + + // control discretization + int num; + + // controls which space to plan in + bool use_pos; + bool use_vel; + bool use_acc; + bool use_jrk; + + // control space + vec_Vec3f U; + + // Update frequency retrieve + private_nh.param("planner_update_frequency", update_frequency, 50.0); + + // max total planning time + private_nh.param("planning_timeout", max_planning_time, 0.150); + + // used for obstacle dilation + private_nh.param("radius", drone_radius, 1.0); + private_nh.param("buffer", obst_buffer, 1.0); + // max x, y, z arena limits - double max_arena_limits[3]; + private_nh.param("/arena/max_x", max_arena_limits[0], 0.0); + private_nh.param("/arena/max_y", max_arena_limits[1], 0.0); + private_nh.param("/arena/max_z", max_arena_limits[2], 0.0); // min x, y, z arena limits - double min_arena_limits[3]; + private_nh.param("/arena/min_x", min_arena_limits[0], 0.0); + private_nh.param("/arena/min_y", min_arena_limits[1], 0.0); + private_nh.param("/arena/min_z", min_arena_limits[2], 0.0); - // Update frequency retrieve - ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); - - // kinematic constraints retrieve - ROS_ASSERT(private_nh.getParam("max_speed", kinematic_constraints[0])); - ROS_ASSERT(private_nh.getParam("max_acceleration", kinematic_constraints[1])); - ROS_ASSERT(private_nh.getParam("max_jerk", kinematic_constraints[2])); - - // arena size/limits retrieve - ROS_ASSERT(private_nh.getParam("/arena/max_x", max_arena_limits[0])); - ROS_ASSERT(private_nh.getParam("/arena/max_y", max_arena_limits[1])); - ROS_ASSERT(private_nh.getParam("/arena/max_z", max_arena_limits[2])); - - ROS_ASSERT(private_nh.getParam("/arena/min_x", min_arena_limits[0])); - ROS_ASSERT(private_nh.getParam("/arena/min_y", min_arena_limits[1])); - ROS_ASSERT(private_nh.getParam("/arena/min_z", min_arena_limits[2])); + // resolution to generate map at + private_nh.param("map_res", map_res, .25); - // Wait for a valid time in case we are using simulated time (not wall time) - while (ros::ok() && ros::Time::now() == ros::Time(0)) { - // wait - ros::spinOnce(); - ros::Duration(.005).sleep(); + // speed, acceleration, jerk limits + private_nh.param("max_speed", kinematic_constraints[0], 0.0); + private_nh.param("max_acceleration", kinematic_constraints[1], 0.0); + private_nh.param("max_jerk", kinematic_constraints[2], 0.0); + + // min/max time discretization for each primitive + private_nh.param("min_dt", min_dt, 0.05); + private_nh.param("max_dt", max_dt, 0.15); + + private_nh.param("use_pos", use_pos, true); + private_nh.param("use_vel", use_vel, true); + private_nh.param("use_acc", use_acc, true); + private_nh.param("use_jrk", use_jrk, false); + + // ndt*dt is the max plan length, in seconds + private_nh.param("ndt", ndt, 1000); + + // Epsilon: greedy param + private_nh.param("eps", eps, 10.0); + + // control discretization + private_nh.param("num", num, 1); + + // maximum number of allowed expansion + private_nh.param("max_num", max_num, 1000); + + // tolerances + private_nh.param("p_tol", pose_tol, .075); + private_nh.param("vel_tol", vel_tol, .1); + private_nh.param("accel_tol", accel_tol, .15); + + // number of samples per second to sample trajectory at + private_nh.param("samples_per_sec", samples_per_sec, 50.0); + + // max jerk is u max + u_max = kinematic_constraints[2]; + + // final obstacle dilation buffer + double buffer = drone_radius + obst_buffer; + + const decimal_t du = u_max / num; + for (decimal_t dx = -u_max; dx <= u_max; dx += du) { + for (decimal_t dy = -u_max; dy <= u_max; dy += du) { + for (decimal_t dz = -u_max; dz <= u_max; dz += du) { + U.push_back(Vec3f(dx, dy, dz)); + } + } } - Server server(nh, "planner_request", false); - server.start(); + iarc7_msgs::PlanGoalConstPtr goal_; + + Point pose_goal, pose_start; + Vector3 twist_goal, twist_start; + Vector3 accel_goal, accel_start; + + // subscribe to obstacle topic + boost::shared_ptr last_msg; + boost::function&)> obstacle_callback = + [&](const boost::shared_ptr& msg) -> void { + if (last_msg == nullptr || last_msg->header.stamp < msg->header.stamp) { + last_msg = msg; + } else { + ROS_ERROR("Motion Planner: bad stamp on obstacle message"); + } + }; + ros::Subscriber obstacle_sub = nh.subscribe("obstacles", 2, obstacle_callback); // Form a connection with the node monitor. If no connection can be made // assert because we don't know what's going on with the other nodes. - ROS_INFO("motion_planner: Attempting to form safety bond"); - + ROS_INFO("Motion_Planner: Attempting to form safety bond"); + Iarc7Safety::SafetyClient safety_client(nh, "motion_planner"); - - ROS_ASSERT_MSG(safety_client.formBond(), - "motion_planner: Could not form bond with safety client"); - // Cache the time - ros::Time last_time = ros::Time::now(); + ROS_ASSERT_MSG(safety_client.formBond(),"Motion_Planner: Could not form bond with safety client"); - // update frequency of the node + // node update rate ros::Rate rate(update_frequency); - PlannerState state = PlannerState::WAITING; - - iarc7_msgs::PlanGoalConstPtr goal_; - - Pose pose_goal; - Twist twist_goal; - Accel accel_goal; - // Run until ROS says we need to shutdown while (ros::ok()) { // Check the safety client before updating anything - // there is no safety response for this node, so - // shut down. - ROS_ASSERT_MSG(!safety_client.isFatalActive(), - "motion_planner: fatal event from safety"); - - ROS_ASSERT_MSG(!safety_client.isSafetyActive(), - "Motion_Planner shutdown due to safety active"); - - // Get the time - ros::Time current_time = ros::Time::now(); - - // Make sure we have a new timestamp. This can be a problem with simulated - // time that does not update with high precision. - if (current_time > last_time) { - last_time = current_time; - - // goal was canceled - if (server.isPreemptRequested() && state == PlannerState::PLANNING) { - server.setPreempted(); - ROS_INFO("Preempt requested. Current planning goal was canceled"); - state = PlannerState::WAITING; - } + // there is no safety response for this node, so + // shut down. + ROS_ASSERT_MSG(!safety_client.isFatalActive(), "MotionPlanner: fatal event from safety"); + ROS_ASSERT_MSG(!safety_client.isSafetyActive(), "MotionPlanner shutdown due to safety active"); + + // goal was canceled + if (server.isPreemptRequested()) { + server.setPreempted(); + ROS_INFO("Preempt requested. Current planning goal was canceled"); + } + if (last_msg == nullptr) { + ROS_DEBUG_THROTTLE(30,"MotionPlanner: No obstacle information available, can't build map"); + } else { // get a new goal from action server if (server.isNewGoalAvailable()) { if (server.isActive()) { - ROS_INFO("New goal received with current goal still running"); + ROS_ERROR("New goal received with current goal still running"); } - // planning goal goal_ = server.acceptNewGoal(); - pose_goal = goal_->goal.motion_point.pose; - twist_goal = goal_->goal.motion_point.twist; - accel_goal = goal_->goal.motion_point.accel; + // offset by start pose so that we never "go out" of arena + double x_offset = max_arena_limits[0]/2-goal_->start.motion_point.pose.position.x; + double y_offset = max_arena_limits[1]/2-goal_->start.motion_point.pose.position.y; - if (!checkGoal(pose_goal, twist_goal, accel_goal, - kinematic_constraints, - max_arena_limits, - min_arena_limits)) { - ROS_ERROR("Planner aborting requested gaol"); - server.setAborted(); - state = PlannerState::WAITING; - } else { - state = PlannerState::PLANNING; - ROS_INFO("New goal accepted by planner"); - } - } + pose_start.x = goal_->start.motion_point.pose.position.x + x_offset; + pose_start.y = goal_->start.motion_point.pose.position.y + y_offset; + pose_start.z = goal_->start.motion_point.pose.position.z; + + twist_start = goal_->start.motion_point.twist.linear; + accel_start = goal_->start.motion_point.accel.linear; - // able to generate a plan - if (state == PlannerState::PLANNING) { - - iarc7_msgs::PlanResult result_; - iarc7_msgs::PlanFeedback feedback_; + pose_goal.x = goal_->goal.motion_point.pose.position.x + x_offset; + pose_goal.y = goal_->goal.motion_point.pose.position.y + y_offset; + pose_goal.z = goal_->goal.motion_point.pose.position.z; - // indicate that planning was successful or not - bool success_ = true; - - // planning calls will go here + twist_goal = goal_->goal.motion_point.twist.linear; + accel_goal = goal_->goal.motion_point.accel.linear; - result_.success = success_; - - server.publishFeedback(feedback_); + if (!checkGoal(pose_goal, twist_goal, accel_goal, + kinematic_constraints, + max_arena_limits, + min_arena_limits)) { + ROS_ERROR("Planner aborting requested goal"); + + iarc7_msgs::PlanResult result_; + result_.success = false; + server.setAborted(result_); + } else { + ROS_DEBUG_THROTTLE(30,"New goal accepted by planner, now plan"); + + ros::WallTime t1 = ros::WallTime::now(); + + planning_ros_msgs::VoxelMap map; + map.resolution = map_res; + map.origin.x = min_arena_limits[0]/map_res; + map.origin.y = min_arena_limits[1]/map_res; + map.origin.z = min_arena_limits[2]/map_res; + map.dim.x = max_arena_limits[0]/map_res; + map.dim.y = max_arena_limits[1]/map_res; + map.dim.z = max_arena_limits[2]/map_res; + std::vector data(map.dim.x * map.dim.y * map.dim.z, 0); + map.data = data; + + std::array voxel_map_origin = + {map.origin.x, map.origin.y, map.origin.z}; + + vec_Vec3f pts; + + for (auto& obstacle : last_msg->obstacles) { + // Map each obstacle to the cloud + float pipe_radius = obstacle.pipe_radius + buffer; + float pipe_x = (obstacle.odom.pose.pose.position.x + x_offset); + float pipe_y = (obstacle.odom.pose.pose.position.y + y_offset); + float px, py, pz; + for (pz = voxel_map_origin[2]; pz <= max_arena_limits[2]; pz += 0.1) { + for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { + for (float r = 0; r < pipe_radius; r += map_res) { + px = r * std::cos(theta) + pipe_x + voxel_map_origin[0]; + py = r * std::sin(theta) + pipe_y + voxel_map_origin[1]; + Vec3f pt(px, py, pz); + pts.push_back(pt); + } + } + } + } + + std::array voxel_map_dim = + {max_arena_limits[0], + max_arena_limits[1], + max_arena_limits[2]}; + + std::unique_ptr voxel_grid( + new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); + + voxel_grid->addCloud(pts); + voxel_grid->getMap(map); + + // Initialize map util + Vec3f ori(map.origin.x, map.origin.y, map.origin.z); + Vec3i dim(map.dim.x, map.dim.y, map.dim.z); + + // map util + std::shared_ptr map_util(new MPL::VoxelMapUtil); + + map_util->setMap(ori, dim, map.data, map_res); + + ROS_DEBUG_THROTTLE(30,"Takes %f sec for building and setting map", + (ros::WallTime::now() - t1).toSec()); + + iarc7_msgs::PlanResult result_; + + Waypoint3 start; + start.pos = Vec3f(pose_start.x, pose_start.y, pose_start.z); + start.vel = Vec3f(twist_start.x, twist_start.y, twist_start.z); + start.acc = Vec3f(accel_start.x, accel_start.y, accel_start.z); + start.use_pos = use_pos; + start.use_vel = use_vel; + start.use_acc = use_acc; + start.use_jrk = use_jrk; + + Waypoint3 goal; + goal.pos = Vec3f(pose_goal.x, pose_goal.y, pose_goal.z); + goal.vel = Vec3f(twist_goal.x, twist_goal.y, twist_goal.z); + goal.acc = Vec3f(accel_goal.x, accel_goal.y, accel_goal.z); + goal.use_pos = use_pos; + goal.use_vel = use_vel; + goal.use_acc = use_acc; + goal.use_jrk = use_jrk; + + bool done_planning = false; + bool valid_plan = false; + + Trajectory<3> traj; + + ros::Time time_start = ros::Time::now(); + ros::Time time_done = time_start + ros::Duration(max_planning_time); + + int i = 0; + double dt = max_dt-i*.05; + + while (dt >= min_dt && ros::ok() && !done_planning) { + std::unique_ptr planner; + planner.reset(new MPMap3DUtil(false)); + planner->setMapUtil(map_util); + planner->setEpsilon(eps); + planner->setVmax(kinematic_constraints[0]); + planner->setAmax(kinematic_constraints[1]); + planner->setUmax(kinematic_constraints[2]); + planner->setDt(dt); + planner->setTmax(ndt*dt); + planner->setMaxNum(max_num); + planner->setU(U); + planner->setTol(pose_tol, vel_tol, accel_tol); + + // now we can plan + ros::Time t0 = ros::Time::now(); + bool valid = planner->plan(start, goal); + ros::Duration planning_time = (ros::Time::now() - t0); + + if (valid) { + traj = planner->getTraj(); + valid_plan = true; + done_planning = ((ros::Time::now() + planning_time) < time_done); + } else { + done_planning = true; + } + + i++; + dt = max_dt-i*.05; + } + + if (!valid_plan) { + ROS_ERROR("Planner failed to find any plan!"); + result_.success = valid_plan; + server.setSucceeded(result_); + } else { + // convert trajectory to motion points + double traj_time = traj.getTotalTime(); + double n_samples = samples_per_sec * traj_time; + + vec_E> waypoints = traj.sample(n_samples); + + ros::Time time_point = goal_->header.stamp; + ros::Duration t_step = ros::Duration(traj_time/waypoints.size()); + + for (Waypoint<3> waypoint : waypoints) { + iarc7_msgs::MotionPointStamped m_point; + m_point.header.frame_id = "map"; + m_point.header.stamp = time_point; + + m_point.motion_point.pose.position.x = waypoint.pos(0)-x_offset; + m_point.motion_point.pose.position.y = waypoint.pos(1)-y_offset; + m_point.motion_point.pose.position.z = waypoint.pos(2); + + m_point.motion_point.twist.linear.x = waypoint.vel(0); + m_point.motion_point.twist.linear.y = waypoint.vel(1); + m_point.motion_point.twist.linear.z = waypoint.vel(2); + + m_point.motion_point.accel.linear.x = waypoint.acc(0); + m_point.motion_point.accel.linear.y = waypoint.acc(1); + m_point.motion_point.accel.linear.z = waypoint.acc(2); + + result_.plan.motion_points.push_back(m_point); + time_point = time_point + t_step; + } + result_.success = valid_plan; + result_.total_time = traj_time; + server.setSucceeded(result_); + } + } + } else { + ROS_DEBUG_THROTTLE(30, "Planner: No new goal."); } } - // Handle all ROS callbacks ros::spinOnce(); rate.sleep(); } - // All is good. return 0; }