From cfab70afab231f1bedf5faececbdbb0a15fe74fd Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Fri, 8 Jun 2018 17:22:32 -0400 Subject: [PATCH 01/26] added new params, renamed param files to keep up with new platforms, moved planner params to a struct, fix node start-up order, misc. fixes --- param/planner.yaml | 6 + .../{platform_1.1.yaml => platform_2.0.yaml} | 7 + param/platform_sim.yaml | 7 + src/MotionPlanner.cpp | 137 +++++++++++++----- 4 files changed, 120 insertions(+), 37 deletions(-) rename param/{platform_1.1.yaml => platform_2.0.yaml} (77%) diff --git a/param/planner.yaml b/param/planner.yaml index 2eded09..54558da 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -19,3 +19,9 @@ use_pos: true use_vel: true use_acc: true use_jrk: false + +dt: 0.2 +ndt: 1000 +eps: 10.0 +num: 3 +max_num: -1 # no limit diff --git a/param/platform_1.1.yaml b/param/platform_2.0.yaml similarity index 77% rename from param/platform_1.1.yaml rename to param/platform_2.0.yaml index c02bfaf..0b8ec53 100644 --- a/param/platform_1.1.yaml +++ b/param/platform_2.0.yaml @@ -7,5 +7,12 @@ max_speed: 3.0 # m/s max_acceleration: 5.0 # m/s^2 max_jerk: 15.0 # m/s^3 +u_max: 20.0 + # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 + +# pos, vel, accel tolerances +p_tol: 0.2 +v_tol: 0.2 +a_tol: 0.5 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index c02bfaf..0b8ec53 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -7,5 +7,12 @@ max_speed: 3.0 # m/s max_acceleration: 5.0 # m/s^2 max_jerk: 15.0 # m/s^3 +u_max: 20.0 + # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 + +# pos, vel, accel tolerances +p_tol: 0.2 +v_tol: 0.2 +a_tol: 0.5 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 3dd25dd..43e15fc 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -24,6 +24,17 @@ #include "geometry_msgs/Twist.h" #include "geometry_msgs/Accel.h" +#include +#include +#include +#include +#include +#include +#include + +#include + + using geometry_msgs::Pose; using geometry_msgs::Twist; using geometry_msgs::Accel; @@ -33,6 +44,29 @@ typedef actionlib::SimpleActionServer Server; enum class PlannerState { WAITING, PLANNING }; +struct PlannerSettings +{ + // min and max arena limits + double max_arena_limits[3]; + double min_arena_limits[3]; + + // speed, acceleration, jerk limits + double kinematic_constraints[3]; + + // pos, vel, accel tolerances + double tolerances[3]; + + double dt; + double eps; + double u_max; + + int max_num; + int num; + int ndt; + + vec_Vec3f U; +}; + bool checkGoal(Pose& pose_goal, Twist& twist_goal, Accel& accel_goal, double kinematic_constraints[3], @@ -40,7 +74,7 @@ bool checkGoal(Pose& pose_goal, Twist& twist_goal, 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]){ + || pose_goal.position.z > max_arena[2]) { ROS_ERROR("Goal provided to planner is above arena limits"); return false; @@ -48,7 +82,7 @@ bool checkGoal(Pose& pose_goal, Twist& twist_goal, if (pose_goal.position.x < min_arena[0] || pose_goal.position.z < min_arena[1] - || pose_goal.position.z < min_arena[2]){ + || pose_goal.position.z < min_arena[2]) { ROS_ERROR("Goal provided to planner is below arena limits"); return false; @@ -88,35 +122,6 @@ int main(int argc, char **argv) // encapsulate parameters ros::NodeHandle private_nh ("~"); - // LOAD PARAMETERS - double update_frequency; - - // speed, acceleration, jerk limits - double kinematic_constraints[3]; - - // max x, y, z arena limits - double max_arena_limits[3]; - - // min x, y, z arena limits - double min_arena_limits[3]; - - // 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])); - // 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 @@ -124,9 +129,6 @@ int main(int argc, char **argv) ros::Duration(.005).sleep(); } - Server server(nh, "planner_request", false); - server.start(); - // 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"); @@ -136,6 +138,65 @@ int main(int argc, char **argv) ROS_ASSERT_MSG(safety_client.formBond(), "motion_planner: Could not form bond with safety client"); + // start action server + Server server(nh, "planner_request", false); + server.start(); + + // LOAD PARAMETERS + + // node update frequency + double update_frequency; + + // Update frequency retrieve + ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); + + // settings for planner + PlannerSettings settings; + + // max x, y, z arena limits + ROS_ASSERT(private_nh.getParam("/arena/max_x", settings.max_arena_limits[0])); + ROS_ASSERT(private_nh.getParam("/arena/max_y", settings.max_arena_limits[1])); + ROS_ASSERT(private_nh.getParam("/arena/max_z", settings.max_arena_limits[2])); + + // min x, y, z arena limits + ROS_ASSERT(private_nh.getParam("/arena/min_x", settings.min_arena_limits[0])); + ROS_ASSERT(private_nh.getParam("/arena/min_y", settings.min_arena_limits[1])); + ROS_ASSERT(private_nh.getParam("/arena/min_z", settings.min_arena_limits[2])); + + // speed, acceleration, jerk limits + ROS_ASSERT(private_nh.getParam("max_speed", settings.kinematic_constraints[0])); + ROS_ASSERT(private_nh.getParam("max_acceleration", settings.kinematic_constraints[1])); + ROS_ASSERT(private_nh.getParam("max_jerk", settings.kinematic_constraints[2])); + + ROS_ASSERT(private_nh.getParam("dt", settings.dt)); + ROS_ASSERT(private_nh.getParam("eps", settings.eps)); + ROS_ASSERT(private_nh.getParam("ndt", settings.ndt)); + + ROS_ASSERT(private_nh.getParam("u_max", settings.u_max)); + ROS_ASSERT(private_nh.getParam("max_num",settings. max_num)); + ROS_ASSERT(private_nh.getParam("num", settings.num)); + + // kinematic tolerances + ROS_ASSERT(private_nh.getParam("p_tol", settings.tolerances[0])); + ROS_ASSERT(private_nh.getParam("v_tol", settings.tolerances[1])); + ROS_ASSERT(private_nh.getParam("a_tol", settings.tolerances[2])); + + vec_Vec3f U; + const decimal_t du = settings.u_max / settings.num; + for (decimal_t dx = -settings.u_max; dx <= settings.u_max; dx += du) { + for (decimal_t dy = -settings.u_max; dy <= settings.u_max; dy += du) { + for (decimal_t dz = -settings.u_max; dz <= settings.u_max; dz += du) { + U.push_back(Vec3f(dx, dy, dz)); + } + } + } + + settings.U = U; + + // debug publishers + ros::Publisher map_pub = nh.advertise("voxel_map", 1, true); + ros::Publisher refined_traj_pub = nh.advertise("trajectory_refined", 1, true); + // Cache the time ros::Time last_time = ros::Time::now(); @@ -190,9 +251,9 @@ int main(int argc, char **argv) accel_goal = goal_->goal.motion_point.accel; if (!checkGoal(pose_goal, twist_goal, accel_goal, - kinematic_constraints, - max_arena_limits, - min_arena_limits)) { + settings.kinematic_constraints, + settings.max_arena_limits, + settings.min_arena_limits)) { ROS_ERROR("Planner aborting requested gaol"); server.setAborted(); state = PlannerState::WAITING; @@ -213,6 +274,8 @@ int main(int argc, char **argv) // planning calls will go here + + result_.success = success_; server.publishFeedback(feedback_); From 46358e1c677d2fe9d3ce38706aac37826214826c Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 1 Jul 2018 14:54:35 -0400 Subject: [PATCH 02/26] WIP --- CMakeLists.txt | 10 +- include/MapGenerator.hpp | 40 ++++ launch/planner.launch | 3 +- launch/rviz.launch | 9 + param/planner.yaml | 27 ++- src/MotionPlanner.cpp | 476 +++++++++++++++++++++++++++++---------- 6 files changed, 431 insertions(+), 134 deletions(-) create mode 100644 include/MapGenerator.hpp create mode 100644 launch/rviz.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index b8b6f9c..4c4ddf1 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_DEBUG"${CMAKE_CXX_FLAGS_DEBUG} -DROS_ASSERT_ENABLED") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -114,9 +116,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..0649de9 100644 --- a/launch/planner.launch +++ b/launch/planner.launch @@ -4,12 +4,11 @@ + - 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/param/planner.yaml b/param/planner.yaml index 54558da..8e6df63 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 -# update frequency +# update frequency planner_update_frequency : 60.0 # The flags use_xxx will adapt the planning in different control space @@ -15,13 +15,30 @@ planner_update_frequency : 60.0 # use_acc = false | use_acc = false | use_acc = true | use_acc = true # use_jrk = false | use_jrk = false | use_jrk = false | use_jrk = true +arena: + max_x: 10.0 # meters + max_y: 10.0 # meters + max_z: 3.0 # meters + + min_x: -10.0 # meters + min_y: -10.0 # meters + min_z: 0.0 # meters + use_pos: true use_vel: true use_acc: true use_jrk: false -dt: 0.2 +# time discretization for each primitive +dt: 0.26 ndt: 1000 eps: 10.0 -num: 3 -max_num: -1 # no limit + +# control discretization +num: 1 + +# maximum number of allowed expansion; -1 means no limit +max_num: 5000 + +# resolution to generate map at +map_res: .25 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 43e15fc..70a0539 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,95 +11,216 @@ //////////////////////////////////////////////////////////////////////////// // 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 -#include +#include "nav_msgs/Odometry.h" + +// MPL message headers #include -#include -#include -#include -#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; typedef actionlib::SimpleActionServer Server; -enum class PlannerState { WAITING, - PLANNING }; +enum class PlannerState { WAITING, PLANNING }; + +void generateVoxelMap(planning_ros_msgs::VoxelMap &voxel_map, + const iarc7_msgs::ObstacleArray &obstacles) { + ros::Time t1 = ros::Time::now(); + + sensor_msgs::PointCloud cloud; + + // Generate cloud from obstacle data + ROS_INFO("Number of obstacles: [%zu]", obstacles.obstacles.size()); + cloud.header.stamp = ros::Time::now(); + cloud.header.frame_id = "cloud"; + cloud.channels.resize(1); + + Vec3f voxel_map_origin; + voxel_map_origin(0) = voxel_map.origin.x; + voxel_map_origin(1) = voxel_map.origin.y; + voxel_map_origin(2) = voxel_map.origin.z; + + ROS_INFO("Mapping obstacles to the cloud"); + for (auto &obstacle : obstacles.obstacles) { + // Map each obstacle to the cloud + + float pipe_radius = obstacle.pipe_radius; + float pipe_height = obstacle.pipe_height; + float pipe_x = obstacle.odom.pose.pose.position.x; + float pipe_y = obstacle.odom.pose.pose.position.y; + float px, py, pz; + for (pz = voxel_map_origin(2); pz <= pipe_height; pz += 0.1) { + for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { + for (float r = pipe_radius - voxel_map.resolution; + r < pipe_radius; r += voxel_map.resolution) { + px = r * cos(theta) + pipe_x + voxel_map_origin(0); + py = r * sin(theta) + pipe_y + voxel_map_origin(1); + geometry_msgs::Point32 point; + point.x = px; + point.y = py; + point.z = pz; + cloud.points.push_back(point); + } + } + } + } -struct PlannerSettings -{ - // min and max arena limits - double max_arena_limits[3]; - double min_arena_limits[3]; + ROS_INFO("Takes %f sec for mapping obstalces", + (ros::Time::now() - t1).toSec()); - // speed, acceleration, jerk limits - double kinematic_constraints[3]; + t1 = ros::Time::now(); - // pos, vel, accel tolerances - double tolerances[3]; + vec_Vec3f pts = cloud_to_vec(cloud); - double dt; - double eps; - double u_max; + ROS_INFO("Takes %f sec for cloud to vec", (ros::Time::now() - t1).toSec()); - int max_num; - int num; - int ndt; + Vec3f voxel_map_dim; + voxel_map_dim(0) = voxel_map.dim.x * voxel_map.resolution; + voxel_map_dim(1) = voxel_map.dim.y * voxel_map.resolution; + voxel_map_dim(2) = voxel_map.dim.z * voxel_map.resolution; - vec_Vec3f U; -}; + std::unique_ptr voxel_grid( + new VoxelGrid(voxel_map_origin, voxel_map_dim, voxel_map.resolution)); + voxel_grid->addCloud(pts); + + voxel_map = voxel_grid->getMap(); + voxel_map.header = cloud.header; +} + +void setMap(std::shared_ptr &map_util, + const planning_ros_msgs::VoxelMap &msg) { + Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z); + Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z); + decimal_t res = msg.resolution; + std::vector map = msg.data; + + map_util->setMap(ori, dim, map, res); +} + +void getMap(std::shared_ptr &map_util, + planning_ros_msgs::VoxelMap &map) { + Vec3f ori = map_util->getOrigin(); + Vec3i dim = map_util->getDim(); + decimal_t res = map_util->getRes(); + + map.origin.x = ori(0); + map.origin.y = ori(1); + map.origin.z = ori(2); + + map.dim.x = dim(0); + map.dim.y = dim(1); + map.dim.z = dim(2); + map.resolution = res; + + map.data = map_util->getMap(); +} + + + +void get_map_util(std::shared_ptr &map_util, + planning_ros_msgs::VoxelMap &map, + double map_res, + double min_arena_limits[3], + double max_arena_limits[3]) { + ros::Time t1 = ros::Time::now(); + + map.resolution = map_res; + + map.origin.x = min_arena_limits[0]; + map.origin.y = min_arena_limits[1]; + map.origin.z = min_arena_limits[2]; + + map.dim.x = max_arena_limits[0]; + map.dim.y = max_arena_limits[1]; + map.dim.z = max_arena_limits[2]; + + std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); + map.data = data; + + iarc7_msgs::ObstacleArray obstacles; + + iarc7_msgs::Obstacle new_obstacle; + new_obstacle.pipe_height = 2.0; + new_obstacle.pipe_radius = 1.0; + new_obstacle.odom.pose.pose.position.x = 9; + new_obstacle.odom.pose.pose.position.y = 11; + new_obstacle.odom.pose.pose.position.z = 0; + obstacles.obstacles.push_back(new_obstacle); + + for (double i = 0; i < 2; i += 0.8) { + new_obstacle.pipe_height = 2.0; + new_obstacle.pipe_radius = 1.0; + new_obstacle.odom.pose.pose.position.x = 15 - i * 2; + new_obstacle.odom.pose.pose.position.y = 15 + 0.5 * i; + new_obstacle.odom.pose.pose.position.z = 0; + obstacles.obstacles.push_back(new_obstacle); + } + + generateVoxelMap(map, obstacles); + + // Initialize map util + setMap(map_util, map); + + // Free unknown space and dilate obstacles + map_util->freeUnknown(); + // map_util->dilate(0.2, 0.1); + + ROS_INFO("Takes %f sec for building map", (ros::Time::now() - t1).toSec()); +} -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(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]) { 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.position.x < min_arena[0] || + pose_goal.position.z < min_arena[1] || + pose_goal.position.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.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"); return false; } - if (std::max({std::abs(accel_goal.linear.x), - std::abs(accel_goal.linear.y), + if (std::max({std::abs(accel_goal.linear.x), std::abs(accel_goal.linear.y), std::abs(accel_goal.linear.z)}) > kinematic_constraints[1]) { - ROS_ERROR("Goal provided to planner is beyond platform acceleration limits"); return false; } @@ -108,8 +229,7 @@ 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"); @@ -117,10 +237,10 @@ int main(int argc, char **argv) // 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)) { @@ -129,15 +249,6 @@ int main(int argc, char **argv) ros::Duration(.005).sleep(); } - // 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"); - - Iarc7Safety::SafetyClient safety_client(nh, "motion_planner"); - - ROS_ASSERT_MSG(safety_client.formBond(), - "motion_planner: Could not form bond with safety client"); - // start action server Server server(nh, "planner_request", false); server.start(); @@ -145,102 +256,169 @@ int main(int argc, char **argv) // LOAD PARAMETERS // node update frequency - double update_frequency; + double update_frequency = 20; // Update frequency retrieve ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); // settings for planner - PlannerSettings settings; + + // min and max arena limits + double max_arena_limits[3] = {20, 20, 20}; + double min_arena_limits[3] = {0, 0, 0}; + + // speed, acceleration, jerk limits + double kinematic_constraints[3] = {3, 5, 15}; + + // pos, vel, accel tolerances + double tolerances[3] = {.2, .2, .5}; + + double dt = .26; + double eps = 10; + double u_max = 20; + + // resolution to generate map at + double map_res = .25; + + int max_num = 5000; + int num = 1; + int ndt = 1000; + + bool use_pos = true; + bool use_vel = true; + bool use_acc = true; + bool use_jrk = false; + + vec_Vec3f U; // max x, y, z arena limits - ROS_ASSERT(private_nh.getParam("/arena/max_x", settings.max_arena_limits[0])); - ROS_ASSERT(private_nh.getParam("/arena/max_y", settings.max_arena_limits[1])); - ROS_ASSERT(private_nh.getParam("/arena/max_z", settings.max_arena_limits[2])); + 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])); // min x, y, z arena limits - ROS_ASSERT(private_nh.getParam("/arena/min_x", settings.min_arena_limits[0])); - ROS_ASSERT(private_nh.getParam("/arena/min_y", settings.min_arena_limits[1])); - ROS_ASSERT(private_nh.getParam("/arena/min_z", settings.min_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 + ROS_ASSERT(private_nh.getParam("map_res", map_res)); // speed, acceleration, jerk limits - ROS_ASSERT(private_nh.getParam("max_speed", settings.kinematic_constraints[0])); - ROS_ASSERT(private_nh.getParam("max_acceleration", settings.kinematic_constraints[1])); - ROS_ASSERT(private_nh.getParam("max_jerk", settings.kinematic_constraints[2])); + 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])); - ROS_ASSERT(private_nh.getParam("dt", settings.dt)); - ROS_ASSERT(private_nh.getParam("eps", settings.eps)); - ROS_ASSERT(private_nh.getParam("ndt", settings.ndt)); + // time discretization for each primitive + ROS_ASSERT(private_nh.getParam("dt", dt)); - ROS_ASSERT(private_nh.getParam("u_max", settings.u_max)); - ROS_ASSERT(private_nh.getParam("max_num",settings. max_num)); - ROS_ASSERT(private_nh.getParam("num", settings.num)); + ROS_ASSERT(private_nh.getParam("use_pos", use_pos)); + ROS_ASSERT(private_nh.getParam("use_vel", use_vel)); + ROS_ASSERT(private_nh.getParam("use_acc", use_acc)); + ROS_ASSERT(private_nh.getParam("use_jrk", use_jrk)); - // kinematic tolerances - ROS_ASSERT(private_nh.getParam("p_tol", settings.tolerances[0])); - ROS_ASSERT(private_nh.getParam("v_tol", settings.tolerances[1])); - ROS_ASSERT(private_nh.getParam("a_tol", settings.tolerances[2])); + // ndt*dt is the plannign horizon + ROS_ASSERT(private_nh.getParam("ndt", ndt)); - vec_Vec3f U; - const decimal_t du = settings.u_max / settings.num; - for (decimal_t dx = -settings.u_max; dx <= settings.u_max; dx += du) { - for (decimal_t dy = -settings.u_max; dy <= settings.u_max; dy += du) { - for (decimal_t dz = -settings.u_max; dz <= settings.u_max; dz += du) { + // Epsilon: greedy param + ROS_ASSERT(private_nh.getParam("eps", eps)); + + // control discretization + ROS_ASSERT(private_nh.getParam("num", num)); + + // max for control space + ROS_ASSERT(private_nh.getParam("u_max", u_max)); + + // maximum number of allowed expansion + ROS_ASSERT(private_nh.getParam("max_num", max_num)); + + // kinematic tolerances + ROS_ASSERT(private_nh.getParam("p_tol", tolerances[0])); + ROS_ASSERT(private_nh.getParam("v_tol", tolerances[1])); + ROS_ASSERT(private_nh.getParam("a_tol", tolerances[2])); + + 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)); } } } - settings.U = U; - // debug publishers ros::Publisher map_pub = nh.advertise("voxel_map", 1, true); - ros::Publisher refined_traj_pub = nh.advertise("trajectory_refined", 1, true); + ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); + ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); // Cache the time ros::Time last_time = ros::Time::now(); - // update frequency of the node - ros::Rate rate(update_frequency); - - PlannerState state = PlannerState::WAITING; - - iarc7_msgs::PlanGoalConstPtr goal_; + iarc7_msgs::PlanGoalConstPtr goal_; Pose pose_goal; Twist twist_goal; Accel accel_goal; + std::shared_ptr map_util(new MPL::VoxelMapUtil); + + // initialize planner + std::unique_ptr planner; + planner.reset(new MPMap3DUtil(true)); + planner->setMapUtil(map_util); + planner->setEpsilon(eps); + planner->setVmax(kinematic_constraints[0]); + planner->setAmax(kinematic_constraints[1]); + planner->setUmax(u_max); + planner->setDt(dt); + planner->setTmax(ndt*dt); + planner->setMaxNum(max_num); + planner->setU(U); + planner->setTol(tolerances[0]); + + // 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"); + // Iarc7Safety::SafetyClient safety_client(nh, "motion_planner"); + // ROS_ASSERT_MSG(safety_client.formBond(),"Motion_Planner: Could not form bond with safety client"); + + // ROS_ERROR("BOND MADE"); + + ros::Rate rate(update_frequency); + + PlannerState state = PlannerState::WAITING; + // 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"); + // there is no safety response for this node, so + // shut down. - ROS_ASSERT_MSG(!safety_client.isSafetyActive(), - "Motion_Planner shutdown due to safety active"); + // 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. + // 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) { + if (server.isPreemptRequested() && + state == PlannerState::PLANNING) { server.setPreempted(); - ROS_INFO("Preempt requested. Current planning goal was canceled"); + ROS_INFO( + "Preempt requested. Current planning goal was canceled"); state = PlannerState::WAITING; } // get a new goal from action server if (server.isNewGoalAvailable()) { if (server.isActive()) { - ROS_INFO("New goal received with current goal still running"); + ROS_INFO( + "New goal received with current goal still running"); } // planning goal @@ -251,9 +429,9 @@ int main(int argc, char **argv) accel_goal = goal_->goal.motion_point.accel; if (!checkGoal(pose_goal, twist_goal, accel_goal, - settings.kinematic_constraints, - settings.max_arena_limits, - settings.min_arena_limits)) { + kinematic_constraints, + max_arena_limits, + min_arena_limits)) { ROS_ERROR("Planner aborting requested gaol"); server.setAborted(); state = PlannerState::WAITING; @@ -265,20 +443,73 @@ int main(int argc, char **argv) // able to generate a plan if (state == PlannerState::PLANNING) { - iarc7_msgs::PlanResult result_; iarc7_msgs::PlanFeedback feedback_; - // indicate that planning was successful or not - bool success_ = true; - - // planning calls will go here + planning_ros_msgs::VoxelMap map; + + std::shared_ptr map_util(new MPL::VoxelMapUtil); + get_map_util(map_util, map, map_res, min_arena_limits, max_arena_limits); + + // Publish the dilated map for visualization + getMap(map_util, map); + map.header.frame_id = "map"; + map_pub.publish(map); + + // Set start and goal + double start_x = 1.0; + double start_y = 1.0; + double start_z = 0.5; + + double start_vx = 0.0; + double start_vy = 0.0; + double start_vz = 0.1; + + Waypoint3 start; + start.pos = Vec3f(start_x, start_y, start_z); + start.vel = Vec3f(start_vx, start_vy, start_vz); + start.acc = Vec3f(0, 0, 0); + 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.position.x, pose_goal.position.y, pose_goal.position.z); + goal.vel = Vec3f(twist_goal.linear.x, twist_goal.linear.y, twist_goal.linear.z); + goal.acc = Vec3f(accel_goal.linear.x, accel_goal.linear.y, accel_goal.linear.z); + goal.use_pos = use_pos; + goal.use_vel = use_vel; + goal.use_acc = use_acc; + goal.use_jrk = use_jrk; + + planner.reset(new MPMap3DUtil(true)); + planner->setMapUtil(map_util); + + // now we can plan + ros::Time t0 = ros::Time::now(); + bool valid = planner->plan(start, goal); + + if (!valid) { + ROS_WARN("Failed and took %f sec for planning, expand [%zu] nodes", + (ros::Time::now() - t0).toSec(), planner->getCloseSet().size()); + } else { + ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", + (ros::Time::now() - t0).toSec(), planner->getCloseSet().size()); + } + // Publish trajectory + auto traj = planner->getTraj(); + planning_ros_msgs::Trajectory traj_msg = toTrajectoryROSMsg(traj); + traj_msg.header.frame_id = "map"; + traj_pub.publish(traj_msg); + result_.success = valid; - result_.success = success_; - server.publishFeedback(feedback_); + + if (valid) + state = PlannerState::WAITING; } } @@ -286,7 +517,6 @@ int main(int argc, char **argv) ros::spinOnce(); rate.sleep(); } - // All is good. return 0; } From f42b615cbad3354d855ad4f2859445945f6f4775 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 1 Jul 2018 16:52:40 -0400 Subject: [PATCH 03/26] WIP 2 --- launch/planner.launch | 3 +- src/MotionPlanner.cpp | 142 ++++++++++++++++++++++-------------------- 2 files changed, 78 insertions(+), 67 deletions(-) diff --git a/launch/planner.launch b/launch/planner.launch index 0649de9..3d56ed7 100644 --- a/launch/planner.launch +++ b/launch/planner.launch @@ -3,7 +3,8 @@ + type="motion_planner" + launch-prefix="valgrind" > diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 70a0539..0cd9a0f 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -47,6 +47,7 @@ typedef actionlib::SimpleActionServer Server; enum class PlannerState { WAITING, PLANNING }; + void generateVoxelMap(planning_ros_msgs::VoxelMap &voxel_map, const iarc7_msgs::ObstacleArray &obstacles) { ros::Time t1 = ros::Time::now(); @@ -141,58 +142,6 @@ void getMap(std::shared_ptr &map_util, -void get_map_util(std::shared_ptr &map_util, - planning_ros_msgs::VoxelMap &map, - double map_res, - double min_arena_limits[3], - double max_arena_limits[3]) { - ros::Time t1 = ros::Time::now(); - - map.resolution = map_res; - - map.origin.x = min_arena_limits[0]; - map.origin.y = min_arena_limits[1]; - map.origin.z = min_arena_limits[2]; - - map.dim.x = max_arena_limits[0]; - map.dim.y = max_arena_limits[1]; - map.dim.z = max_arena_limits[2]; - - std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); - map.data = data; - - iarc7_msgs::ObstacleArray obstacles; - - iarc7_msgs::Obstacle new_obstacle; - new_obstacle.pipe_height = 2.0; - new_obstacle.pipe_radius = 1.0; - new_obstacle.odom.pose.pose.position.x = 9; - new_obstacle.odom.pose.pose.position.y = 11; - new_obstacle.odom.pose.pose.position.z = 0; - obstacles.obstacles.push_back(new_obstacle); - - for (double i = 0; i < 2; i += 0.8) { - new_obstacle.pipe_height = 2.0; - new_obstacle.pipe_radius = 1.0; - new_obstacle.odom.pose.pose.position.x = 15 - i * 2; - new_obstacle.odom.pose.pose.position.y = 15 + 0.5 * i; - new_obstacle.odom.pose.pose.position.z = 0; - obstacles.obstacles.push_back(new_obstacle); - } - - generateVoxelMap(map, obstacles); - - // Initialize map util - setMap(map_util, map); - - // Free unknown space and dilate obstacles - map_util->freeUnknown(); - // map_util->dilate(0.2, 0.1); - - ROS_INFO("Takes %f sec for building map", (ros::Time::now() - t1).toSec()); -} - - bool checkGoal(Pose &pose_goal, Twist &twist_goal, Accel &accel_goal, @@ -351,16 +300,28 @@ int main(int argc, char **argv) { ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); - // Cache the time - ros::Time last_time = ros::Time::now(); + std::shared_ptr map_util(new MPL::VoxelMapUtil); - iarc7_msgs::PlanGoalConstPtr goal_; - Pose pose_goal; - Twist twist_goal; - Accel accel_goal; + planning_ros_msgs::VoxelMap map; - std::shared_ptr map_util(new MPL::VoxelMapUtil); + ros::Time start_map = ros::Time::now(); + + map.resolution = map_res; + + map.origin.x = min_arena_limits[0]; + map.origin.y = min_arena_limits[1]; + map.origin.z = min_arena_limits[2]; + + map.dim.x = max_arena_limits[0]; + map.dim.y = max_arena_limits[1]; + map.dim.z = max_arena_limits[2]; + + std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); + map.data = data; + + // Initialize map util + setMap(map_util, map); // initialize planner std::unique_ptr planner; @@ -376,13 +337,22 @@ int main(int argc, char **argv) { planner->setU(U); planner->setTol(tolerances[0]); + iarc7_msgs::PlanGoalConstPtr goal_; + + Pose pose_goal; + Twist twist_goal; + Accel accel_goal; + // 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"); + // Iarc7Safety::SafetyClient safety_client(nh, "motion_planner"); + // ROS_ASSERT_MSG(safety_client.formBond(),"Motion_Planner: Could not form bond with safety client"); - // ROS_ERROR("BOND MADE"); + // Cache the time + ros::Time last_time = ros::Time::now(); ros::Rate rate(update_frequency); @@ -446,10 +416,50 @@ int main(int argc, char **argv) { iarc7_msgs::PlanResult result_; iarc7_msgs::PlanFeedback feedback_; - planning_ros_msgs::VoxelMap map; + ros::Time start_map = ros::Time::now(); + + map.resolution = map_res; + + map.origin.x = min_arena_limits[0]; + map.origin.y = min_arena_limits[1]; + map.origin.z = min_arena_limits[2]; + + map.dim.x = max_arena_limits[0]; + map.dim.y = max_arena_limits[1]; + map.dim.z = max_arena_limits[2]; + + std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); + map.data = data; - std::shared_ptr map_util(new MPL::VoxelMapUtil); - get_map_util(map_util, map, map_res, min_arena_limits, max_arena_limits); + iarc7_msgs::ObstacleArray obstacles; + + iarc7_msgs::Obstacle new_obstacle; + new_obstacle.pipe_height = 2.0; + new_obstacle.pipe_radius = 1.0; + new_obstacle.odom.pose.pose.position.x = 9; + new_obstacle.odom.pose.pose.position.y = 11; + new_obstacle.odom.pose.pose.position.z = 0; + obstacles.obstacles.push_back(new_obstacle); + + for (double i = 0; i < 2; i += 0.8) { + new_obstacle.pipe_height = 2.0; + new_obstacle.pipe_radius = 1.0; + new_obstacle.odom.pose.pose.position.x = 15 - i * 2; + new_obstacle.odom.pose.pose.position.y = 15 + 0.5 * i; + new_obstacle.odom.pose.pose.position.z = 0; + obstacles.obstacles.push_back(new_obstacle); + } + + generateVoxelMap(map, obstacles); + + // Initialize map util + setMap(map_util, map); + + // Free unknown space and dilate obstacles + map_util->freeUnknown(); + // map_util->dilate(0.2, 0.1); + + ROS_INFO("Takes %f sec for building map", (ros::Time::now() - start_map).toSec()); // Publish the dilated map for visualization getMap(map_util, map); @@ -483,8 +493,9 @@ int main(int argc, char **argv) { goal.use_acc = use_acc; goal.use_jrk = use_jrk; - planner.reset(new MPMap3DUtil(true)); planner->setMapUtil(map_util); + planner->setU(U); + planner->setUmax(u_max); // now we can plan ros::Time t0 = ros::Time::now(); @@ -508,8 +519,7 @@ int main(int argc, char **argv) { server.publishFeedback(feedback_); - if (valid) - state = PlannerState::WAITING; + state = PlannerState::WAITING; } } From 8bc7bb9815cffb86bec1dcfa6768347722e9a790 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Wed, 4 Jul 2018 03:23:44 -0400 Subject: [PATCH 04/26] WIP 3 --- CMakeLists.txt | 1 - launch/planner.launch | 3 +- package.xml | 3 +- src/MotionPlanner.cpp | 442 +++++++++++++++++++----------------------- 4 files changed, 201 insertions(+), 248 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4c4ddf1..6f98bcc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,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 ) diff --git a/launch/planner.launch b/launch/planner.launch index 3d56ed7..ce079b6 100644 --- a/launch/planner.launch +++ b/launch/planner.launch @@ -3,8 +3,7 @@ + type="motion_planner" > 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/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 0cd9a0f..5ab5456 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -21,6 +21,8 @@ #include "geometry_msgs/Accel.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/Twist.h" +#include "geometry_msgs/Point.h" +#include "geometry_msgs/Vector3.h" #include "nav_msgs/Odometry.h" @@ -42,134 +44,40 @@ using iarc7_msgs::MotionPointStamped; using geometry_msgs::Accel; using geometry_msgs::Pose; using geometry_msgs::Twist; +using geometry_msgs::Vector3; +using geometry_msgs::Point; typedef actionlib::SimpleActionServer Server; enum class PlannerState { WAITING, PLANNING }; -void generateVoxelMap(planning_ros_msgs::VoxelMap &voxel_map, - const iarc7_msgs::ObstacleArray &obstacles) { - ros::Time t1 = ros::Time::now(); - - sensor_msgs::PointCloud cloud; - - // Generate cloud from obstacle data - ROS_INFO("Number of obstacles: [%zu]", obstacles.obstacles.size()); - cloud.header.stamp = ros::Time::now(); - cloud.header.frame_id = "cloud"; - cloud.channels.resize(1); - - Vec3f voxel_map_origin; - voxel_map_origin(0) = voxel_map.origin.x; - voxel_map_origin(1) = voxel_map.origin.y; - voxel_map_origin(2) = voxel_map.origin.z; - - ROS_INFO("Mapping obstacles to the cloud"); - for (auto &obstacle : obstacles.obstacles) { - // Map each obstacle to the cloud - - float pipe_radius = obstacle.pipe_radius; - float pipe_height = obstacle.pipe_height; - float pipe_x = obstacle.odom.pose.pose.position.x; - float pipe_y = obstacle.odom.pose.pose.position.y; - float px, py, pz; - for (pz = voxel_map_origin(2); pz <= pipe_height; pz += 0.1) { - for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = pipe_radius - voxel_map.resolution; - r < pipe_radius; r += voxel_map.resolution) { - px = r * cos(theta) + pipe_x + voxel_map_origin(0); - py = r * sin(theta) + pipe_y + voxel_map_origin(1); - geometry_msgs::Point32 point; - point.x = px; - point.y = py; - point.z = pz; - cloud.points.push_back(point); - } - } - } - } - - ROS_INFO("Takes %f sec for mapping obstalces", - (ros::Time::now() - t1).toSec()); - - t1 = ros::Time::now(); - - vec_Vec3f pts = cloud_to_vec(cloud); - - ROS_INFO("Takes %f sec for cloud to vec", (ros::Time::now() - t1).toSec()); - - Vec3f voxel_map_dim; - voxel_map_dim(0) = voxel_map.dim.x * voxel_map.resolution; - voxel_map_dim(1) = voxel_map.dim.y * voxel_map.resolution; - voxel_map_dim(2) = voxel_map.dim.z * voxel_map.resolution; - - std::unique_ptr voxel_grid( - new VoxelGrid(voxel_map_origin, voxel_map_dim, voxel_map.resolution)); - voxel_grid->addCloud(pts); - - voxel_map = voxel_grid->getMap(); - voxel_map.header = cloud.header; -} - -void setMap(std::shared_ptr &map_util, - const planning_ros_msgs::VoxelMap &msg) { - Vec3f ori(msg.origin.x, msg.origin.y, msg.origin.z); - Vec3i dim(msg.dim.x, msg.dim.y, msg.dim.z); - decimal_t res = msg.resolution; - std::vector map = msg.data; - - map_util->setMap(ori, dim, map, res); -} - -void getMap(std::shared_ptr &map_util, - planning_ros_msgs::VoxelMap &map) { - Vec3f ori = map_util->getOrigin(); - Vec3i dim = map_util->getDim(); - decimal_t res = map_util->getRes(); - - map.origin.x = ori(0); - map.origin.y = ori(1); - map.origin.z = ori(2); - - map.dim.x = dim(0); - map.dim.y = dim(1); - map.dim.z = dim(2); - map.resolution = res; - - map.data = map_util->getMap(); -} - - - - - -bool checkGoal(Pose &pose_goal, Twist &twist_goal, Accel &accel_goal, +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.position.x > max_arena[0] || - pose_goal.position.y > max_arena[1] || - pose_goal.position.z > max_arena[2]) { + 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]) { + 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; } @@ -198,7 +106,7 @@ int main(int argc, char **argv) { ros::Duration(.005).sleep(); } - // start action server + // start action server Server server(nh, "planner_request", false); server.start(); @@ -295,65 +203,36 @@ int main(int argc, char **argv) { } } - // debug publishers - ros::Publisher map_pub = nh.advertise("voxel_map", 1, true); - ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); - ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); - - std::shared_ptr map_util(new MPL::VoxelMapUtil); - - - planning_ros_msgs::VoxelMap map; - - ros::Time start_map = ros::Time::now(); - - map.resolution = map_res; - - map.origin.x = min_arena_limits[0]; - map.origin.y = min_arena_limits[1]; - map.origin.z = min_arena_limits[2]; - - map.dim.x = max_arena_limits[0]; - map.dim.y = max_arena_limits[1]; - map.dim.z = max_arena_limits[2]; - - std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); - map.data = data; + iarc7_msgs::PlanGoalConstPtr goal_; - // Initialize map util - setMap(map_util, map); + Point pose_goal, pose_start; + Vector3 twist_goal, twist_start; + Vector3 accel_goal, accel_start; - // initialize planner - std::unique_ptr planner; - planner.reset(new MPMap3DUtil(true)); - planner->setMapUtil(map_util); - planner->setEpsilon(eps); - planner->setVmax(kinematic_constraints[0]); - planner->setAmax(kinematic_constraints[1]); - planner->setUmax(u_max); - planner->setDt(dt); - planner->setTmax(ndt*dt); - planner->setMaxNum(max_num); - planner->setU(U); - planner->setTol(tolerances[0]); + // for caching maps + bool new_obstacle_available = true; + planning_ros_msgs::VoxelMap last_map; - iarc7_msgs::PlanGoalConstPtr goal_; + // map util + std::shared_ptr map_util(new MPL::VoxelMapUtil); - Pose pose_goal; - Twist twist_goal; - Accel accel_goal; + // debug publishers + ros::Publisher map_pub = nh.advertise("voxel_map", 1, true); + ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); + ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); // 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"); + Iarc7Safety::SafetyClient safety_client(nh, "motion_planner"); - // ROS_ASSERT_MSG(safety_client.formBond(),"Motion_Planner: Could not form bond with safety client"); + 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(); + // node update rate ros::Rate rate(update_frequency); PlannerState state = PlannerState::WAITING; @@ -364,8 +243,8 @@ int main(int argc, char **argv) { // 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"); + 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(); @@ -384,118 +263,193 @@ int main(int argc, char **argv) { state = PlannerState::WAITING; } - // get a new goal from action server - if (server.isNewGoalAvailable()) { - if (server.isActive()) { - ROS_INFO( - "New goal received with current goal still running"); - } - - // planning goal - goal_ = server.acceptNewGoal(); + if (state == PlannerState::WAITING) { + // get a new goal from action server + if (server.isNewGoalAvailable()) { + if (server.isActive()) { + ROS_INFO("New goal received with current goal still running"); + } + + // planning goal + goal_ = server.acceptNewGoal(); + + pose_goal = goal_->goal.motion_point.pose.position; + twist_goal = goal_->goal.motion_point.twist.linear; + accel_goal = goal_->goal.motion_point.accel.linear; + + pose_start = goal_->start.motion_point.pose.position; + twist_start = goal_->start.motion_point.twist.linear; + accel_start = goal_->start.motion_point.accel.linear; + + 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 { + ROS_INFO("New goal accepted by planner, now map"); + } - pose_goal = goal_->goal.motion_point.pose; - twist_goal = goal_->goal.motion_point.twist; - accel_goal = goal_->goal.motion_point.accel; + state = PlannerState::PLANNING; - 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"); + ROS_DEBUG_THROTTLE(60, "Planner: No new goal. Generate map only"); + state = PlannerState::WAITING; } - } - // able to generate a plan - if (state == PlannerState::PLANNING) { - iarc7_msgs::PlanResult result_; - iarc7_msgs::PlanFeedback feedback_; + if (new_obstacle_available) { + iarc7_msgs::ObstacleArray obstacles; - ros::Time start_map = ros::Time::now(); - - map.resolution = map_res; - - map.origin.x = min_arena_limits[0]; - map.origin.y = min_arena_limits[1]; - map.origin.z = min_arena_limits[2]; - - map.dim.x = max_arena_limits[0]; - map.dim.y = max_arena_limits[1]; - map.dim.z = max_arena_limits[2]; - - std::vector data(map.dim.x*map.dim.y*map.dim.z, 0); - map.data = data; - - iarc7_msgs::ObstacleArray obstacles; - - iarc7_msgs::Obstacle new_obstacle; - new_obstacle.pipe_height = 2.0; - new_obstacle.pipe_radius = 1.0; - new_obstacle.odom.pose.pose.position.x = 9; - new_obstacle.odom.pose.pose.position.y = 11; - new_obstacle.odom.pose.pose.position.z = 0; - obstacles.obstacles.push_back(new_obstacle); - - for (double i = 0; i < 2; i += 0.8) { + iarc7_msgs::Obstacle new_obstacle; new_obstacle.pipe_height = 2.0; new_obstacle.pipe_radius = 1.0; - new_obstacle.odom.pose.pose.position.x = 15 - i * 2; - new_obstacle.odom.pose.pose.position.y = 15 + 0.5 * i; + new_obstacle.odom.pose.pose.position.x = 9; + new_obstacle.odom.pose.pose.position.y = 11; new_obstacle.odom.pose.pose.position.z = 0; obstacles.obstacles.push_back(new_obstacle); - } - - generateVoxelMap(map, obstacles); - // Initialize map util - setMap(map_util, map); - - // Free unknown space and dilate obstacles - map_util->freeUnknown(); - // map_util->dilate(0.2, 0.1); + for (double i = 0; i < 2; i += 0.8) { + new_obstacle.pipe_height = 1.5; + new_obstacle.pipe_radius = 0.7; + new_obstacle.odom.pose.pose.position.x = 9 - i * 2; + new_obstacle.odom.pose.pose.position.y = 9 + 0.5 * i; + new_obstacle.odom.pose.pose.position.z = 0; + obstacles.obstacles.push_back(new_obstacle); + } + + ros::Time t1 = ros::Time::now(); + + // Standard header + std_msgs::Header header; + header.frame_id = std::string("map"); + + 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; + + sensor_msgs::PointCloud cloud; + + // Generate cloud from obstacle data + ROS_INFO("Number of obstacles: [%zu]", obstacles.obstacles.size()); + cloud.header.stamp = ros::Time::now(); + cloud.header.frame_id = "cloud"; + cloud.channels.resize(1); + + std::array voxel_map_origin = + {map.origin.x, map.origin.y, map.origin.z}; + + ROS_INFO("Mapping obstacles to the cloud"); + for (auto& obstacle : obstacles.obstacles) { + // Map each obstacle to the cloud + float pipe_radius = obstacle.pipe_radius; + float pipe_height = obstacle.pipe_height; + float pipe_x = obstacle.odom.pose.pose.position.x; + float pipe_y = obstacle.odom.pose.pose.position.y; + float px, py, pz; + for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { + for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { + for (float r = pipe_radius - map_res; 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]; + geometry_msgs::Point32 point; + point.x = px; + point.y = py; + point.z = pz; + cloud.points.push_back(point); + } + } + } + } + + vec_Vec3f pts = cloud_to_vec(cloud); + + std::array voxel_map_dim = + {map.dim.x * map_res, + map.dim.y * map_res, + map.dim.z * map_res}; + + std::unique_ptr voxel_grid( + new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); + + voxel_grid->addCloud(pts); + voxel_grid->getMap(map); + + map.header = cloud.header; + + // 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->setMap(ori, dim, map.data, map_res); + + // Free unknown space and dilate obstacles + // map_util->freeUnknown(); + // map_util->dilate(0.2, 0.1); + + ROS_INFO("Takes %f sec for building map", (ros::Time::now() - t1).toSec()); + + // Publish the dilated map for visualization + map.header = header; + map_pub.publish(map); + + last_map = map; + + new_obstacle_available = false; + } else { + ROS_INFO("MotionPlanner: No new obstacle info available. Using previously generated map"); - ROS_INFO("Takes %f sec for building map", (ros::Time::now() - start_map).toSec()); + Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); + Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); - // Publish the dilated map for visualization - getMap(map_util, map); - map.header.frame_id = "map"; - map_pub.publish(map); + map_util->setMap(ori, dim, last_map.data, map_res); + } - // Set start and goal - double start_x = 1.0; - double start_y = 1.0; - double start_z = 0.5; + } - double start_vx = 0.0; - double start_vy = 0.0; - double start_vz = 0.1; + // able to generate a plan + if (state == PlannerState::PLANNING) { + iarc7_msgs::PlanResult result_; + iarc7_msgs::PlanFeedback feedback_; Waypoint3 start; - start.pos = Vec3f(start_x, start_y, start_z); - start.vel = Vec3f(start_vx, start_vy, start_vz); - start.acc = Vec3f(0, 0, 0); + 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.position.x, pose_goal.position.y, pose_goal.position.z); - goal.vel = Vec3f(twist_goal.linear.x, twist_goal.linear.y, twist_goal.linear.z); - goal.acc = Vec3f(accel_goal.linear.x, accel_goal.linear.y, accel_goal.linear.z); + 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; + std::unique_ptr planner; + planner.reset(new MPMap3DUtil(true)); 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->setUmax(u_max); + planner->setTol(tolerances[0]); // now we can plan ros::Time t0 = ros::Time::now(); @@ -509,11 +463,13 @@ int main(int argc, char **argv) { (ros::Time::now() - t0).toSec(), planner->getCloseSet().size()); } - // Publish trajectory - auto traj = planner->getTraj(); - planning_ros_msgs::Trajectory traj_msg = toTrajectoryROSMsg(traj); - traj_msg.header.frame_id = "map"; - traj_pub.publish(traj_msg); + if (valid) { + // Publish trajectory + auto traj = planner->getTraj(); + planning_ros_msgs::Trajectory traj_msg = toTrajectoryROSMsg(traj); + traj_msg.header.frame_id = "map"; + traj_pub.publish(traj_msg); + } result_.success = valid; From 3839fa6f1d69e4439acbb05b134ed5fcf926ec0b Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 5 Jul 2018 22:08:18 -0400 Subject: [PATCH 05/26] fixes and works --- param/planner.yaml | 8 ++--- param/platform_2.0.yaml | 5 ++- param/platform_sim.yaml | 5 ++- src/MotionPlanner.cpp | 80 ++++++++++++++++++++++------------------- 4 files changed, 55 insertions(+), 43 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 8e6df63..77e92bf 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -16,12 +16,12 @@ planner_update_frequency : 60.0 # use_jrk = false | use_jrk = false | use_jrk = false | use_jrk = true arena: - max_x: 10.0 # meters - max_y: 10.0 # meters + max_x: 20.0 # meters + max_y: 20.0 # meters max_z: 3.0 # meters - min_x: -10.0 # meters - min_y: -10.0 # meters + min_x: 0.0 # meters + min_y: 0.0 # meters min_z: 0.0 # meters use_pos: true diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 0b8ec53..c782a72 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -10,9 +10,12 @@ max_jerk: 15.0 # m/s^3 u_max: 20.0 # Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +min_maneuver_height: 0.3 # pos, vel, accel tolerances p_tol: 0.2 v_tol: 0.2 a_tol: 0.5 + +# offset to account for coordinate frame differences +obstacle_coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index 0b8ec53..c782a72 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -10,9 +10,12 @@ max_jerk: 15.0 # m/s^3 u_max: 20.0 # Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +min_maneuver_height: 0.3 # pos, vel, accel tolerances p_tol: 0.2 v_tol: 0.2 a_tol: 0.5 + +# offset to account for coordinate frame differences +obstacle_coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 5ab5456..9e65ef4 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -53,7 +53,8 @@ enum class PlannerState { WAITING, PLANNING }; bool checkGoal(Point &pose_goal, Vector3 &twist_goal, Vector3 &accel_goal, - double kinematic_constraints[3], double max_arena[3], + 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] || @@ -115,9 +116,6 @@ int main(int argc, char **argv) { // node update frequency double update_frequency = 20; - // Update frequency retrieve - ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); - // settings for planner // min and max arena limits @@ -137,6 +135,8 @@ int main(int argc, char **argv) { // resolution to generate map at double map_res = .25; + double obstacle_coordinate_offset = 10; + int max_num = 5000; int num = 1; int ndt = 1000; @@ -148,6 +148,9 @@ int main(int argc, char **argv) { vec_Vec3f U; + // Update frequency retrieve + ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); + // max x, y, z arena limits ROS_ASSERT(private_nh.getParam("arena/max_x", max_arena_limits[0])); ROS_ASSERT(private_nh.getParam("arena/max_y", max_arena_limits[1])); @@ -194,6 +197,9 @@ int main(int argc, char **argv) { ROS_ASSERT(private_nh.getParam("v_tol", tolerances[1])); ROS_ASSERT(private_nh.getParam("a_tol", tolerances[2])); + // correction for obstacle-planner coordinate frame differences + ROS_ASSERT(private_nh.getParam("obstacle_coordinate_offset", obstacle_coordinate_offset)); + 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) { @@ -210,7 +216,7 @@ int main(int argc, char **argv) { Vector3 accel_goal, accel_start; // for caching maps - bool new_obstacle_available = true; + bool new_obstacle_available = false; planning_ros_msgs::VoxelMap last_map; // map util @@ -221,6 +227,21 @@ int main(int argc, char **argv) { ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); + // 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; + new_obstacle_available = true; + + } else { + ROS_ERROR("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"); @@ -263,7 +284,11 @@ int main(int argc, char **argv) { state = PlannerState::WAITING; } - if (state == PlannerState::WAITING) { + if (last_msg == nullptr) { + ROS_ERROR("MotionPlanner: No obstacle information available, can't build map"); + new_obstacle_available = false; + state = PlannerState::WAITING; + } else if (state == PlannerState::WAITING) { // get a new goal from action server if (server.isNewGoalAvailable()) { if (server.isActive()) { @@ -300,26 +325,7 @@ int main(int argc, char **argv) { } if (new_obstacle_available) { - iarc7_msgs::ObstacleArray obstacles; - - iarc7_msgs::Obstacle new_obstacle; - new_obstacle.pipe_height = 2.0; - new_obstacle.pipe_radius = 1.0; - new_obstacle.odom.pose.pose.position.x = 9; - new_obstacle.odom.pose.pose.position.y = 11; - new_obstacle.odom.pose.pose.position.z = 0; - obstacles.obstacles.push_back(new_obstacle); - - for (double i = 0; i < 2; i += 0.8) { - new_obstacle.pipe_height = 1.5; - new_obstacle.pipe_radius = 0.7; - new_obstacle.odom.pose.pose.position.x = 9 - i * 2; - new_obstacle.odom.pose.pose.position.y = 9 + 0.5 * i; - new_obstacle.odom.pose.pose.position.z = 0; - obstacles.obstacles.push_back(new_obstacle); - } - - ros::Time t1 = ros::Time::now(); + ros::WallTime t1 = ros::WallTime::now(); // Standard header std_msgs::Header header; @@ -339,7 +345,7 @@ int main(int argc, char **argv) { sensor_msgs::PointCloud cloud; // Generate cloud from obstacle data - ROS_INFO("Number of obstacles: [%zu]", obstacles.obstacles.size()); + ROS_INFO("Number of obstacles: [%zu]", last_msg->obstacles.size()); cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = "cloud"; cloud.channels.resize(1); @@ -348,12 +354,14 @@ int main(int argc, char **argv) { {map.origin.x, map.origin.y, map.origin.z}; ROS_INFO("Mapping obstacles to the cloud"); - for (auto& obstacle : obstacles.obstacles) { + for (auto& obstacle : last_msg->obstacles) { // Map each obstacle to the cloud float pipe_radius = obstacle.pipe_radius; float pipe_height = obstacle.pipe_height; - float pipe_x = obstacle.odom.pose.pose.position.x; - float pipe_y = obstacle.odom.pose.pose.position.y; + float pipe_x = (obstacle.odom.pose.pose.position.x + + obstacle_coordinate_offset)/map_res; + float pipe_y = (obstacle.odom.pose.pose.position.y + + obstacle_coordinate_offset)/map_res; float px, py, pz; for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { @@ -395,7 +403,7 @@ int main(int argc, char **argv) { // map_util->freeUnknown(); // map_util->dilate(0.2, 0.1); - ROS_INFO("Takes %f sec for building map", (ros::Time::now() - t1).toSec()); + ROS_INFO("Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization map.header = header; @@ -404,7 +412,7 @@ int main(int argc, char **argv) { last_map = map; new_obstacle_available = false; - } else { + } else if (last_msg != nullptr) { ROS_INFO("MotionPlanner: No new obstacle info available. Using previously generated map"); Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); @@ -412,7 +420,6 @@ int main(int argc, char **argv) { map_util->setMap(ori, dim, last_map.data, map_res); } - } // able to generate a plan @@ -452,15 +459,14 @@ int main(int argc, char **argv) { planner->setTol(tolerances[0]); // now we can plan - ros::Time t0 = ros::Time::now(); + ros::WallTime t0 = ros::WallTime::now(); bool valid = planner->plan(start, goal); if (!valid) { - ROS_WARN("Failed and took %f sec for planning, expand [%zu] nodes", - (ros::Time::now() - t0).toSec(), planner->getCloseSet().size()); + ROS_WARN("Failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); } else { ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", - (ros::Time::now() - t0).toSec(), planner->getCloseSet().size()); + (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); } if (valid) { From 3e24492232900809f60a802321534fcbb400ba78 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 8 Jul 2018 01:22:28 -0400 Subject: [PATCH 06/26] IT WORKS! --- param/planner.yaml | 3 +++ src/MotionPlanner.cpp | 63 ++++++++++++++++++++++++++++++++----------- 2 files changed, 51 insertions(+), 15 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 77e92bf..26d67c4 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -42,3 +42,6 @@ max_num: 5000 # resolution to generate map at map_res: .25 + +# samples per sec for generating waypoints +samples_per_sec: 50 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 9e65ef4..06b5f82 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -135,6 +135,9 @@ int main(int argc, char **argv) { // resolution to generate map at double map_res = .25; + double samples_per_sec = 50; + + // samples per sec for generating waypoints double obstacle_coordinate_offset = 10; int max_num = 5000; @@ -177,7 +180,7 @@ int main(int argc, char **argv) { ROS_ASSERT(private_nh.getParam("use_acc", use_acc)); ROS_ASSERT(private_nh.getParam("use_jrk", use_jrk)); - // ndt*dt is the plannign horizon + // ndt*dt is the planning horizon ROS_ASSERT(private_nh.getParam("ndt", ndt)); // Epsilon: greedy param @@ -200,6 +203,8 @@ int main(int argc, char **argv) { // correction for obstacle-planner coordinate frame differences ROS_ASSERT(private_nh.getParam("obstacle_coordinate_offset", obstacle_coordinate_offset)); + ROS_ASSERT(private_nh.getParam("samples_per_sec", samples_per_sec)); + 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) { @@ -345,7 +350,7 @@ int main(int argc, char **argv) { sensor_msgs::PointCloud cloud; // Generate cloud from obstacle data - ROS_INFO("Number of obstacles: [%zu]", last_msg->obstacles.size()); + //ROS_INFO("Number of obstacles: [%zu]", last_msg->obstacles.size()); cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = "cloud"; cloud.channels.resize(1); @@ -353,7 +358,7 @@ int main(int argc, char **argv) { std::array voxel_map_origin = {map.origin.x, map.origin.y, map.origin.z}; - ROS_INFO("Mapping obstacles to the cloud"); + //ROS_INFO("Mapping obstacles to the cloud"); for (auto& obstacle : last_msg->obstacles) { // Map each obstacle to the cloud float pipe_radius = obstacle.pipe_radius; @@ -403,7 +408,7 @@ int main(int argc, char **argv) { // map_util->freeUnknown(); // map_util->dilate(0.2, 0.1); - ROS_INFO("Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); + //ROS_INFO("Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization map.header = header; @@ -413,7 +418,7 @@ int main(int argc, char **argv) { new_obstacle_available = false; } else if (last_msg != nullptr) { - ROS_INFO("MotionPlanner: No new obstacle info available. Using previously generated map"); + //ROS_INFO("MotionPlanner: No new obstacle info available. Using previously generated map"); Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); @@ -464,24 +469,52 @@ int main(int argc, char **argv) { if (!valid) { ROS_WARN("Failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); + ROS_ERROR("Planner failed to find a plan, will try again"); + + feedback_.success = valid; + server.publishFeedback(feedback_); } else { ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); - } - if (valid) { - // Publish trajectory + // convert trajectory to motion points auto traj = planner->getTraj(); - planning_ros_msgs::Trajectory traj_msg = toTrajectoryROSMsg(traj); - traj_msg.header.frame_id = "map"; - traj_pub.publish(traj_msg); - } - result_.success = valid; + double traj_time = traj.getTotalTime(); + double n_samples = samples_per_sec * traj_time; - server.publishFeedback(feedback_); + vec_E> waypoints = traj.sample(n_samples); - state = PlannerState::WAITING; + ros::Time time_point = ros::Time::now(); + 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); + m_point.motion_point.pose.position.y = waypoint.pos(1); + 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; + result_.total_time = traj_time; + + server.setSucceeded(result_); + + state = PlannerState::WAITING; + } } } From 77f8372b9c0ce9c5ce3cc0e9fa670889806980d2 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 8 Jul 2018 17:29:32 -0400 Subject: [PATCH 07/26] made some fixes --- param/planner.yaml | 2 +- param/platform_2.0.yaml | 2 +- param/platform_sim.yaml | 2 +- src/MotionPlanner.cpp | 36 ++++++++++++++++-------------------- 4 files changed, 19 insertions(+), 23 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 26d67c4..e00a8fd 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -30,7 +30,7 @@ use_acc: true use_jrk: false # time discretization for each primitive -dt: 0.26 +dt: 0.20 ndt: 1000 eps: 10.0 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index c782a72..ee2dae6 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -18,4 +18,4 @@ v_tol: 0.2 a_tol: 0.5 # offset to account for coordinate frame differences -obstacle_coordinate_offset: 10 +coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index c782a72..ee2dae6 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -18,4 +18,4 @@ v_tol: 0.2 a_tol: 0.5 # offset to account for coordinate frame differences -obstacle_coordinate_offset: 10 +coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 06b5f82..46cd5e0 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -135,10 +135,10 @@ int main(int argc, char **argv) { // resolution to generate map at double map_res = .25; + // samples per sec for generating waypoints double samples_per_sec = 50; - // samples per sec for generating waypoints - double obstacle_coordinate_offset = 10; + double coordinate_offset = 10; int max_num = 5000; int num = 1; @@ -201,7 +201,7 @@ int main(int argc, char **argv) { ROS_ASSERT(private_nh.getParam("a_tol", tolerances[2])); // correction for obstacle-planner coordinate frame differences - ROS_ASSERT(private_nh.getParam("obstacle_coordinate_offset", obstacle_coordinate_offset)); + ROS_ASSERT(private_nh.getParam("coordinate_offset", coordinate_offset)); ROS_ASSERT(private_nh.getParam("samples_per_sec", samples_per_sec)); @@ -284,8 +284,7 @@ int main(int argc, char **argv) { if (server.isPreemptRequested() && state == PlannerState::PLANNING) { server.setPreempted(); - ROS_INFO( - "Preempt requested. Current planning goal was canceled"); + ROS_INFO("Preempt requested. Current planning goal was canceled"); state = PlannerState::WAITING; } @@ -364,9 +363,9 @@ int main(int argc, char **argv) { float pipe_radius = obstacle.pipe_radius; float pipe_height = obstacle.pipe_height; float pipe_x = (obstacle.odom.pose.pose.position.x + - obstacle_coordinate_offset)/map_res; + coordinate_offset)/map_res; float pipe_y = (obstacle.odom.pose.pose.position.y + - obstacle_coordinate_offset)/map_res; + coordinate_offset)/map_res; float px, py, pz; for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { @@ -408,7 +407,7 @@ int main(int argc, char **argv) { // map_util->freeUnknown(); // map_util->dilate(0.2, 0.1); - //ROS_INFO("Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); + ROS_DEBUG_THROTTLE(60, "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization map.header = header; @@ -418,7 +417,7 @@ int main(int argc, char **argv) { new_obstacle_available = false; } else if (last_msg != nullptr) { - //ROS_INFO("MotionPlanner: No new obstacle info available. Using previously generated map"); + ROS_DEBUG_THROTTLE(60, "MotionPlanner: No new obstacle info available. Using previously generated map"); Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); @@ -451,7 +450,7 @@ int main(int argc, char **argv) { goal.use_jrk = use_jrk; std::unique_ptr planner; - planner.reset(new MPMap3DUtil(true)); + planner.reset(new MPMap3DUtil(false)); planner->setMapUtil(map_util); planner->setEpsilon(eps); planner->setVmax(kinematic_constraints[0]); @@ -485,7 +484,7 @@ int main(int argc, char **argv) { vec_E> waypoints = traj.sample(n_samples); - ros::Time time_point = ros::Time::now(); + ros::Time time_point = goal_->header.stamp; ros::Duration t_step = ros::Duration(traj_time/waypoints.size()); for (Waypoint<3> waypoint : waypoints) { @@ -493,8 +492,8 @@ int main(int argc, char **argv) { m_point.header.frame_id = "map"; m_point.header.stamp = time_point; - m_point.motion_point.pose.position.x = waypoint.pos(0); - m_point.motion_point.pose.position.y = waypoint.pos(1); + m_point.motion_point.pose.position.x = waypoint.pos(0)-coordinate_offset; + m_point.motion_point.pose.position.y = waypoint.pos(1)-coordinate_offset; m_point.motion_point.pose.position.z = waypoint.pos(2); m_point.motion_point.twist.linear.x = waypoint.vel(0); @@ -505,15 +504,12 @@ int main(int argc, char **argv) { 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); + feedback_.plan.motion_points.push_back(m_point); time_point = time_point + t_step; } - result_.success = valid; - result_.total_time = traj_time; - - server.setSucceeded(result_); - - state = PlannerState::WAITING; + feedback_.success = valid; + feedback_.total_time = traj_time; + server.publishFeedback(feedback_); } } } From c5b395dfffa518028ccced15f41a8482baae01e3 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Mon, 9 Jul 2018 00:57:26 -0400 Subject: [PATCH 08/26] Testing --- param/planner.yaml | 2 +- src/MotionPlanner.cpp | 13 +++++-------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index e00a8fd..a62ff9f 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -18,7 +18,7 @@ planner_update_frequency : 60.0 arena: max_x: 20.0 # meters max_y: 20.0 # meters - max_z: 3.0 # meters + max_z: 4.0 # meters min_x: 0.0 # meters min_y: 0.0 # meters diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 46cd5e0..34bf5ed 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -370,12 +370,12 @@ int main(int argc, char **argv) { for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { for (float r = pipe_radius - map_res; 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]; + px = r * std::cos(theta) + pipe_x + voxel_map_origin[0] + .5; + py = r * std::sin(theta) + pipe_y + voxel_map_origin[1] + .5; geometry_msgs::Point32 point; point.x = px; point.y = py; - point.z = pz; + point.z = pz + .5; cloud.points.push_back(point); } } @@ -403,11 +403,7 @@ int main(int argc, char **argv) { map_util->setMap(ori, dim, map.data, map_res); - // Free unknown space and dilate obstacles - // map_util->freeUnknown(); - // map_util->dilate(0.2, 0.1); - - ROS_DEBUG_THROTTLE(60, "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); + ROS_INFO( "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization map.header = header; @@ -509,6 +505,7 @@ int main(int argc, char **argv) { } feedback_.success = valid; feedback_.total_time = traj_time; + ROS_ERROR("SENDING FEEDBACK NOW: %f", ros::WallTime::now().toSec()); server.publishFeedback(feedback_); } } From 87013e02956f91891f04caa901545343210cb18e Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Tue, 10 Jul 2018 02:53:20 -0400 Subject: [PATCH 09/26] testing and fixes --- param/planner.yaml | 2 +- param/platform_2.0.yaml | 6 +- param/platform_sim.yaml | 6 +- src/MotionPlanner.cpp | 410 +++++++++++++++++++--------------------- 4 files changed, 201 insertions(+), 223 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index a62ff9f..8f06aa9 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -4,7 +4,7 @@ transform_timeout : 0.2 # update frequency -planner_update_frequency : 60.0 +planner_update_frequency : 50.0 # The flags use_xxx will adapt the planning in different control space # taken from motion primitive library README diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index ee2dae6..4d980cd 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -12,10 +12,8 @@ u_max: 20.0 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 -# pos, vel, accel tolerances -p_tol: 0.2 -v_tol: 0.2 -a_tol: 0.5 +# pose tolerances +p_tol: 0.05 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index ee2dae6..4d980cd 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -12,10 +12,8 @@ u_max: 20.0 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 -# pos, vel, accel tolerances -p_tol: 0.2 -v_tol: 0.2 -a_tol: 0.5 +# pose tolerances +p_tol: 0.05 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 34bf5ed..59e66e8 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -51,7 +51,6 @@ typedef actionlib::SimpleActionServer Server; enum class PlannerState { WAITING, PLANNING }; - bool checkGoal(Point &pose_goal, Vector3 &twist_goal, Vector3 &accel_goal, double kinematic_constraints[3], double max_arena[3], @@ -125,8 +124,8 @@ int main(int argc, char **argv) { // speed, acceleration, jerk limits double kinematic_constraints[3] = {3, 5, 15}; - // pos, vel, accel tolerances - double tolerances[3] = {.2, .2, .5}; + // pose tolerances + double pose_tol = .05; double dt = .26; double eps = 10; @@ -195,10 +194,8 @@ int main(int argc, char **argv) { // maximum number of allowed expansion ROS_ASSERT(private_nh.getParam("max_num", max_num)); - // kinematic tolerances - ROS_ASSERT(private_nh.getParam("p_tol", tolerances[0])); - ROS_ASSERT(private_nh.getParam("v_tol", tolerances[1])); - ROS_ASSERT(private_nh.getParam("a_tol", tolerances[2])); + // position tolerances + ROS_ASSERT(private_nh.getParam("p_tol", pose_tol)); // correction for obstacle-planner coordinate frame differences ROS_ASSERT(private_nh.getParam("coordinate_offset", coordinate_offset)); @@ -255,9 +252,6 @@ int main(int argc, char **argv) { 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(); - // node update rate ros::Rate rate(update_frequency); @@ -265,6 +259,7 @@ int main(int argc, char **argv) { // Run until ROS says we need to shutdown while (ros::ok()) { + ROS_ERROR("PLANNER NODE RESTARTING UP"); // Check the safety client before updating anything // there is no safety response for this node, so // shut down. @@ -272,245 +267,232 @@ int main(int argc, char **argv) { 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(); + // goal was canceled + if (server.isPreemptRequested() && + state == PlannerState::PLANNING) { + server.setPreempted(); + ROS_INFO("Preempt requested. Current planning goal was canceled"); + state = PlannerState::WAITING; + } - // 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; + if (last_msg == nullptr) { + ROS_ERROR("MotionPlanner: No obstacle information available, can't build map"); + new_obstacle_available = false; + state = PlannerState::WAITING; + } else if (state == PlannerState::WAITING) { + // get a new goal from action server + if (server.isNewGoalAvailable()) { + if (server.isActive()) { + ROS_ERROR("New goal received with current goal still running"); + } + // planning goal + goal_ = server.acceptNewGoal(); + + pose_goal = goal_->goal.motion_point.pose.position; + twist_goal = goal_->goal.motion_point.twist.linear; + accel_goal = goal_->goal.motion_point.accel.linear; + + pose_start = goal_->start.motion_point.pose.position; + twist_start = goal_->start.motion_point.twist.linear; + accel_start = goal_->start.motion_point.accel.linear; + + 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 { + ROS_INFO("New goal accepted by planner, now map"); + } - // goal was canceled - if (server.isPreemptRequested() && - state == PlannerState::PLANNING) { - server.setPreempted(); - ROS_INFO("Preempt requested. Current planning goal was canceled"); - state = PlannerState::WAITING; - } + state = PlannerState::PLANNING; - if (last_msg == nullptr) { - ROS_ERROR("MotionPlanner: No obstacle information available, can't build map"); - new_obstacle_available = false; + } else { + ROS_DEBUG_THROTTLE(30, "Planner: No new goal. Generate map only"); state = PlannerState::WAITING; - } else if (state == PlannerState::WAITING) { - // get a new goal from action server - if (server.isNewGoalAvailable()) { - if (server.isActive()) { - ROS_INFO("New goal received with current goal still running"); - } - - // planning goal - goal_ = server.acceptNewGoal(); - - pose_goal = goal_->goal.motion_point.pose.position; - twist_goal = goal_->goal.motion_point.twist.linear; - accel_goal = goal_->goal.motion_point.accel.linear; - - pose_start = goal_->start.motion_point.pose.position; - twist_start = goal_->start.motion_point.twist.linear; - accel_start = goal_->start.motion_point.accel.linear; - - 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 { - ROS_INFO("New goal accepted by planner, now map"); - } - - state = PlannerState::PLANNING; - - } else { - ROS_DEBUG_THROTTLE(60, "Planner: No new goal. Generate map only"); - state = PlannerState::WAITING; - } + } - if (new_obstacle_available) { - ros::WallTime t1 = ros::WallTime::now(); - - // Standard header - std_msgs::Header header; - header.frame_id = std::string("map"); - - 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; - - sensor_msgs::PointCloud cloud; - - // Generate cloud from obstacle data - //ROS_INFO("Number of obstacles: [%zu]", last_msg->obstacles.size()); - cloud.header.stamp = ros::Time::now(); - cloud.header.frame_id = "cloud"; - cloud.channels.resize(1); - - std::array voxel_map_origin = - {map.origin.x, map.origin.y, map.origin.z}; - - //ROS_INFO("Mapping obstacles to the cloud"); - for (auto& obstacle : last_msg->obstacles) { - // Map each obstacle to the cloud - float pipe_radius = obstacle.pipe_radius; - float pipe_height = obstacle.pipe_height; - float pipe_x = (obstacle.odom.pose.pose.position.x + - coordinate_offset)/map_res; - float pipe_y = (obstacle.odom.pose.pose.position.y + - coordinate_offset)/map_res; - float px, py, pz; - for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { - for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = pipe_radius - map_res; r < pipe_radius; r += map_res) { - px = r * std::cos(theta) + pipe_x + voxel_map_origin[0] + .5; - py = r * std::sin(theta) + pipe_y + voxel_map_origin[1] + .5; - geometry_msgs::Point32 point; - point.x = px; - point.y = py; - point.z = pz + .5; - cloud.points.push_back(point); - } + if (new_obstacle_available) { + ros::WallTime t1 = ros::WallTime::now(); + + // Standard header + std_msgs::Header header; + header.frame_id = std::string("map"); + + 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; + + sensor_msgs::PointCloud cloud; + + // Generate cloud from obstacle data + cloud.header.stamp = ros::Time::now(); + cloud.header.frame_id = "cloud"; + cloud.channels.resize(1); + + std::array voxel_map_origin = + {map.origin.x, map.origin.y, map.origin.z}; + + for (auto& obstacle : last_msg->obstacles) { + // Map each obstacle to the cloud + float pipe_radius = obstacle.pipe_radius; + float pipe_height = obstacle.pipe_height; + float pipe_x = (obstacle.odom.pose.pose.position.x + + coordinate_offset)/map_res; + float pipe_y = (obstacle.odom.pose.pose.position.y + + coordinate_offset)/map_res; + float px, py, pz; + for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { + for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { + for (float r = pipe_radius - map_res; r < pipe_radius; r += map_res) { + px = r * std::cos(theta) + pipe_x + voxel_map_origin[0] + .5; + py = r * std::sin(theta) + pipe_y + voxel_map_origin[1] + .5; + geometry_msgs::Point32 point; + point.x = px; + point.y = py; + point.z = pz + .5; + cloud.points.push_back(point); } } } + } - vec_Vec3f pts = cloud_to_vec(cloud); + vec_Vec3f pts = cloud_to_vec(cloud); - std::array voxel_map_dim = - {map.dim.x * map_res, - map.dim.y * map_res, - map.dim.z * map_res}; + std::array voxel_map_dim = + {map.dim.x * map_res, + map.dim.y * map_res, + map.dim.z * map_res}; - std::unique_ptr voxel_grid( - new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); + std::unique_ptr voxel_grid( + new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); - voxel_grid->addCloud(pts); - voxel_grid->getMap(map); + voxel_grid->addCloud(pts); + voxel_grid->getMap(map); - map.header = cloud.header; + map.header = cloud.header; - // 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); + // 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->setMap(ori, dim, map.data, map_res); + map_util->setMap(ori, dim, map.data, map_res); - ROS_INFO( "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); + ROS_DEBUG_THROTTLE(30, "Takes %f sec for building map", + (ros::WallTime::now() - t1).toSec()); - // Publish the dilated map for visualization - map.header = header; - map_pub.publish(map); + // Publish the dilated map for visualization + map.header = header; + map_pub.publish(map); - last_map = map; + last_map = map; - new_obstacle_available = false; - } else if (last_msg != nullptr) { - ROS_DEBUG_THROTTLE(60, "MotionPlanner: No new obstacle info available. Using previously generated map"); + new_obstacle_available = false; + } else if (last_msg != nullptr) { + ROS_DEBUG_THROTTLE(30, "MotionPlanner: No new obstacle info available. Using previously generated map"); - Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); - Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); + Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); + Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); - map_util->setMap(ori, dim, last_map.data, map_res); - } + map_util->setMap(ori, dim, last_map.data, map_res); } + } - // able to generate a plan - if (state == PlannerState::PLANNING) { - iarc7_msgs::PlanResult result_; - iarc7_msgs::PlanFeedback feedback_; - - 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; - - 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(tolerances[0]); - - // now we can plan - ros::WallTime t0 = ros::WallTime::now(); - bool valid = planner->plan(start, goal); - - if (!valid) { - ROS_WARN("Failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); - ROS_ERROR("Planner failed to find a plan, will try again"); - - feedback_.success = valid; - server.publishFeedback(feedback_); - } else { - ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", - (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); + // able to generate a plan + if (state == PlannerState::PLANNING) { + 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; + + 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); + + // now we can plan + ros::WallTime t0 = ros::WallTime::now(); + bool valid = planner->plan(start, goal); + + if (!valid) { + ROS_WARN("Planner failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); + result_.success = valid; + server.setSucceeded(result_); + } else { + ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", + (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); - // convert trajectory to motion points - auto traj = planner->getTraj(); + // convert trajectory to motion points + auto traj = planner->getTraj(); - double traj_time = traj.getTotalTime(); - double n_samples = samples_per_sec * traj_time; + double traj_time = traj.getTotalTime(); + double n_samples = samples_per_sec * traj_time; - vec_E> waypoints = traj.sample(n_samples); + vec_E> waypoints = traj.sample(n_samples); - ros::Time time_point = goal_->header.stamp; - ros::Duration t_step = ros::Duration(traj_time/waypoints.size()); + 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; + 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)-coordinate_offset; - m_point.motion_point.pose.position.y = waypoint.pos(1)-coordinate_offset; - m_point.motion_point.pose.position.z = waypoint.pos(2); + m_point.motion_point.pose.position.x = waypoint.pos(0)-coordinate_offset; + m_point.motion_point.pose.position.y = waypoint.pos(1)-coordinate_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.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); + 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); - feedback_.plan.motion_points.push_back(m_point); - time_point = time_point + t_step; - } - feedback_.success = valid; - feedback_.total_time = traj_time; - ROS_ERROR("SENDING FEEDBACK NOW: %f", ros::WallTime::now().toSec()); - server.publishFeedback(feedback_); + result_.plan.motion_points.push_back(m_point); + time_point = time_point + t_step; } + result_.success = valid; + result_.total_time = traj_time; + ROS_ERROR("SETTING SUCEEDED IN MP"); + server.setSucceeded(result_); + state = PlannerState::WAITING; } } - + ROS_ERROR("UPDATE IS DONE"); // Handle all ROS callbacks ros::spinOnce(); rate.sleep(); From 218833698e312ca7e8788c1e1d0abb687e33e002 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Wed, 11 Jul 2018 00:12:14 -0400 Subject: [PATCH 10/26] removing debug statements --- src/MotionPlanner.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 59e66e8..b678204 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -259,7 +259,6 @@ int main(int argc, char **argv) { // Run until ROS says we need to shutdown while (ros::ok()) { - ROS_ERROR("PLANNER NODE RESTARTING UP"); // Check the safety client before updating anything // there is no safety response for this node, so // shut down. @@ -300,7 +299,7 @@ int main(int argc, char **argv) { kinematic_constraints, max_arena_limits, min_arena_limits)) { - ROS_ERROR("Planner aborting requested gaol"); + ROS_ERROR("Planner aborting requested goal"); server.setAborted(); state = PlannerState::WAITING; } else { @@ -487,12 +486,10 @@ int main(int argc, char **argv) { } result_.success = valid; result_.total_time = traj_time; - ROS_ERROR("SETTING SUCEEDED IN MP"); server.setSucceeded(result_); state = PlannerState::WAITING; } } - ROS_ERROR("UPDATE IS DONE"); // Handle all ROS callbacks ros::spinOnce(); rate.sleep(); From babe02b82cfdae0f59d18f0508f4694b459d38ad Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Wed, 11 Jul 2018 23:51:07 -0400 Subject: [PATCH 11/26] fixed map generation --- CMakeLists.txt | 2 +- src/MotionPlanner.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6f98bcc..cc8c449 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(iarc7_planner) set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror ${CMAKE_CXX_FLAGS}") -set(CMAKE_CXX_FLAGS_DEBUG"${CMAKE_CXX_FLAGS_DEBUG} -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) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index b678204..48fe4fe 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -303,7 +303,7 @@ int main(int argc, char **argv) { server.setAborted(); state = PlannerState::WAITING; } else { - ROS_INFO("New goal accepted by planner, now map"); + ROS_DEBUG_THROTTLE(30,"New goal accepted by planner, now map"); } state = PlannerState::PLANNING; @@ -346,19 +346,19 @@ int main(int argc, char **argv) { float pipe_radius = obstacle.pipe_radius; float pipe_height = obstacle.pipe_height; float pipe_x = (obstacle.odom.pose.pose.position.x + - coordinate_offset)/map_res; + coordinate_offset); float pipe_y = (obstacle.odom.pose.pose.position.y + - coordinate_offset)/map_res; + coordinate_offset); float px, py, pz; for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = pipe_radius - map_res; r < pipe_radius; r += map_res) { + for (float r = 0; r < pipe_radius; r += map_res) { px = r * std::cos(theta) + pipe_x + voxel_map_origin[0] + .5; py = r * std::sin(theta) + pipe_y + voxel_map_origin[1] + .5; geometry_msgs::Point32 point; point.x = px; point.y = py; - point.z = pz + .5; + point.z = std::min(max_arena_limits[2], pz + .5); cloud.points.push_back(point); } } @@ -386,7 +386,7 @@ int main(int argc, char **argv) { map_util->setMap(ori, dim, map.data, map_res); - ROS_DEBUG_THROTTLE(30, "Takes %f sec for building map", + ROS_INFO("Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization From c048a7a403de4ed19d8e58676e7bd7b6c3f28251 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 12 Jul 2018 00:20:00 -0400 Subject: [PATCH 12/26] added crazyflie params, fixed bug --- param/platform_crazyflie.yaml | 19 +++++++++++++++++++ src/MotionPlanner.cpp | 2 -- 2 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 param/platform_crazyflie.yaml diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml new file mode 100644 index 0000000..4d980cd --- /dev/null +++ b/param/platform_crazyflie.yaml @@ -0,0 +1,19 @@ +# 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 + +u_max: 20.0 + +# Minimum height for any XY translationanl maneuvers, meters +min_maneuver_height: 0.3 + +# pose tolerances +p_tol: 0.05 + +# offset to account for coordinate frame differences +coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 48fe4fe..7e35f04 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -447,8 +447,6 @@ int main(int argc, char **argv) { if (!valid) { ROS_WARN("Planner failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); - result_.success = valid; - server.setSucceeded(result_); } else { ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); From a16961bcc4813a3f0b444c7c0c0ae3a552d29a7a Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 12 Jul 2018 00:56:06 -0400 Subject: [PATCH 13/26] fixed logging --- src/MotionPlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 7e35f04..b0cd599 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -386,7 +386,7 @@ int main(int argc, char **argv) { map_util->setMap(ori, dim, map.data, map_res); - ROS_INFO("Takes %f sec for building map", + ROS_DEBUG_THROTTLE(30, "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); // Publish the dilated map for visualization From d8373cc0bc9509e2bd774763b43503ecf5fb71c5 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 12 Jul 2018 19:37:24 -0400 Subject: [PATCH 14/26] testing --- param/planner.yaml | 2 +- param/platform_crazyflie.yaml | 2 +- src/MotionPlanner.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 8f06aa9..3713c8b 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -30,7 +30,7 @@ use_acc: true use_jrk: false # time discretization for each primitive -dt: 0.20 +dt: 0.10 ndt: 1000 eps: 10.0 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index 4d980cd..bf9c134 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -13,7 +13,7 @@ u_max: 20.0 min_maneuver_height: 0.3 # pose tolerances -p_tol: 0.05 +p_tol: 0.08 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index b0cd599..e33f123 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -353,8 +353,8 @@ int main(int argc, char **argv) { for (pz = voxel_map_origin[2]; pz <= pipe_height; 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] + .5; - py = r * std::sin(theta) + pipe_y + voxel_map_origin[1] + .5; + px = r * std::cos(theta) + pipe_x + voxel_map_origin[0]; + py = r * std::sin(theta) + pipe_y + voxel_map_origin[1]; geometry_msgs::Point32 point; point.x = px; point.y = py; From cda60929fe8a7627ddc791d03eb5bcd28335ae33 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Wed, 18 Jul 2018 23:41:30 -0400 Subject: [PATCH 15/26] implemented changing dt, misc. fixes and improvements --- param/planner.yaml | 23 +++-- param/platform_2.0.yaml | 6 +- param/platform_crazyflie.yaml | 8 +- param/platform_sim.yaml | 6 +- src/MotionPlanner.cpp | 170 +++++++++++++++++++++------------- 5 files changed, 124 insertions(+), 89 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 3713c8b..cb8b5f9 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -3,6 +3,9 @@ # Timeout for transforms transform_timeout : 0.2 +# timeout for getting a successful plan +planning_timeout: .300 + # update frequency planner_update_frequency : 50.0 @@ -15,30 +18,26 @@ planner_update_frequency : 50.0 # use_acc = false | use_acc = false | use_acc = true | use_acc = true # use_jrk = false | use_jrk = false | use_jrk = false | use_jrk = true -arena: - max_x: 20.0 # meters - max_y: 20.0 # meters - max_z: 4.0 # meters - - min_x: 0.0 # meters - min_y: 0.0 # meters - min_z: 0.0 # meters - use_pos: true use_vel: true use_acc: true use_jrk: false -# time discretization for each primitive -dt: 0.10 +# min/max time discretization for each primitive +min_dt: 0.05 +max_dt: 0.15 + +# 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: 5000 +max_num: 1000 # resolution to generate map at map_res: .25 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 4d980cd..2c4a13b 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,14 +1,12 @@ # Params for drone size for planner -radius: 1.5 # meters +radius: 0.625 # meters buffer: 0.2 # meters # kinematic constraints max_speed: 3.0 # m/s -max_acceleration: 5.0 # m/s^2 +max_acceleration: 3.0 # m/s^2 max_jerk: 15.0 # m/s^3 -u_max: 20.0 - # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index bf9c134..d55f790 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -3,11 +3,9 @@ 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 - -u_max: 20.0 +max_speed: 2.0 # m/s +max_acceleration: 3.0 # m/s^2 +max_jerk: 10.0 # m/s^3 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index 4d980cd..2c4a13b 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -1,14 +1,12 @@ # Params for drone size for planner -radius: 1.5 # meters +radius: 0.625 # meters buffer: 0.2 # meters # kinematic constraints max_speed: 3.0 # m/s -max_acceleration: 5.0 # m/s^2 +max_acceleration: 3.0 # m/s^2 max_jerk: 15.0 # m/s^3 -u_max: 20.0 - # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index e33f123..c26052b 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -90,7 +90,7 @@ 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; @@ -110,26 +110,47 @@ int main(int argc, char **argv) { Server server(nh, "planner_request", false); server.start(); - // LOAD PARAMETERS + /******** LOAD PARAMETERS ********/ // node update frequency - double update_frequency = 20; + double update_frequency = 50.0; - // settings for planner + // offset for coordinate frame differences with HLM + double coordinate_offset = 10.0; + + // max total planning time + double max_planning_time = .300; + + // radius of drone in meters + double drone_radius = .625; + + // safety buffer to stay away from obstacles + double obst_buffer = .20; + + /******** SETTINGS FOR PLANNER ********/ // min and max arena limits - double max_arena_limits[3] = {20, 20, 20}; + double max_arena_limits[3] = {20, 20, 4}; double min_arena_limits[3] = {0, 0, 0}; // speed, acceleration, jerk limits - double kinematic_constraints[3] = {3, 5, 15}; + double kinematic_constraints[3] = {3, 3, 15}; // pose tolerances double pose_tol = .05; - double dt = .26; + // min and max time discretization + double min_dt = .05; + double max_dt = .15; + + // max time steps + int ndt = 1000; + + // greedy param, epsilon double eps = 10; - double u_max = 20; + + // max for control space + double u_max = 15; // resolution to generate map at double map_res = .25; @@ -137,31 +158,40 @@ int main(int argc, char **argv) { // samples per sec for generating waypoints double samples_per_sec = 50; - double coordinate_offset = 10; - + // max num of expansions int max_num = 5000; + + // control discretization int num = 1; - int ndt = 1000; + // controls which space to plan in bool use_pos = true; bool use_vel = true; bool use_acc = true; bool use_jrk = false; + // control space vec_Vec3f U; // Update frequency retrieve ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); + // max total planning time + ROS_ASSERT(private_nh.getParam("planning_timeout", max_planning_time)); + + // used for obstacle dilation + ROS_ASSERT(private_nh.getParam("radius", drone_radius)); + ROS_ASSERT(private_nh.getParam("buffer", obst_buffer)); + // max x, y, z arena limits - 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/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])); // min x, y, z arena limits - 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])); + 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 ROS_ASSERT(private_nh.getParam("map_res", map_res)); @@ -171,15 +201,16 @@ int main(int argc, char **argv) { ROS_ASSERT(private_nh.getParam("max_acceleration", kinematic_constraints[1])); ROS_ASSERT(private_nh.getParam("max_jerk", kinematic_constraints[2])); - // time discretization for each primitive - ROS_ASSERT(private_nh.getParam("dt", dt)); + // min/max time discretization for each primitive + ROS_ASSERT(private_nh.getParam("min_dt", min_dt)); + ROS_ASSERT(private_nh.getParam("max_dt", max_dt)); ROS_ASSERT(private_nh.getParam("use_pos", use_pos)); ROS_ASSERT(private_nh.getParam("use_vel", use_vel)); ROS_ASSERT(private_nh.getParam("use_acc", use_acc)); ROS_ASSERT(private_nh.getParam("use_jrk", use_jrk)); - // ndt*dt is the planning horizon + // ndt*dt is the max plan length, in seconds ROS_ASSERT(private_nh.getParam("ndt", ndt)); // Epsilon: greedy param @@ -188,9 +219,6 @@ int main(int argc, char **argv) { // control discretization ROS_ASSERT(private_nh.getParam("num", num)); - // max for control space - ROS_ASSERT(private_nh.getParam("u_max", u_max)); - // maximum number of allowed expansion ROS_ASSERT(private_nh.getParam("max_num", max_num)); @@ -200,8 +228,15 @@ int main(int argc, char **argv) { // correction for obstacle-planner coordinate frame differences ROS_ASSERT(private_nh.getParam("coordinate_offset", coordinate_offset)); + // number of samples per second to sample trajectory at ROS_ASSERT(private_nh.getParam("samples_per_sec", samples_per_sec)); + // 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) { @@ -224,13 +259,7 @@ int main(int argc, char **argv) { // map util std::shared_ptr map_util(new MPL::VoxelMapUtil); - // debug publishers - ros::Publisher map_pub = nh.advertise("voxel_map", 1, true); - ros::Publisher traj_pub = nh.advertise("trajectory", 1, true); - ros::Publisher cloud_pub = nh.advertise("cloud", 1, true); - // subscribe to obstacle topic - boost::shared_ptr last_msg; boost::function&)> obstacle_callback = [&](const boost::shared_ptr& msg) -> void { @@ -239,7 +268,7 @@ int main(int argc, char **argv) { new_obstacle_available = true; } else { - ROS_ERROR("Bad stamp on obstacle message"); + ROS_ERROR("Motion Planner: bad stamp on obstacle message"); } }; ros::Subscriber obstacle_sub = nh.subscribe("obstacles", 2, obstacle_callback); @@ -262,9 +291,8 @@ int main(int argc, char **argv) { // 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"); + 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() && @@ -352,13 +380,13 @@ int main(int argc, char **argv) { float px, py, pz; for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = 0; r < pipe_radius; r += map_res) { + for (float r = 0; r < (pipe_radius + buffer); 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]; geometry_msgs::Point32 point; point.x = px; point.y = py; - point.z = std::min(max_arena_limits[2], pz + .5); + point.z = std::min(max_arena_limits[2], pz + buffer); cloud.points.push_back(point); } } @@ -389,10 +417,6 @@ int main(int argc, char **argv) { ROS_DEBUG_THROTTLE(30, "Takes %f sec for building map", (ros::WallTime::now() - t1).toSec()); - // Publish the dilated map for visualization - map.header = header; - map_pub.publish(map); - last_map = map; new_obstacle_available = false; @@ -428,32 +452,50 @@ int main(int argc, char **argv) { goal.use_acc = use_acc; goal.use_jrk = use_jrk; - 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); - - // now we can plan - ros::WallTime t0 = ros::WallTime::now(); - bool valid = planner->plan(start, goal); - - if (!valid) { - ROS_WARN("Planner failed and took %f sec for planning", (ros::WallTime::now() - t0).toSec()); - } else { - ROS_INFO("Succeeded and took %f sec for planning, expand [%zu] nodes", - (ros::WallTime::now() - t0).toSec(), planner->getCloseSet().size()); + double dt = max_dt; + 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); + + 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); + + // 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; + } - // convert trajectory to motion points - auto traj = planner->getTraj(); + done_planning = (done_planning && ((ros::Time::now() + planning_time) < time_done)); + dt = dt - .05; + } + if (!valid_plan) { + ROS_ERROR("Planner failed to find any plan!"); + result_.success = valid_plan; + server.setSucceeded(result_); + state = PlannerState::WAITING; + } else { + // convert trajectory to motion points double traj_time = traj.getTotalTime(); double n_samples = samples_per_sec * traj_time; @@ -482,7 +524,7 @@ int main(int argc, char **argv) { result_.plan.motion_points.push_back(m_point); time_point = time_point + t_step; } - result_.success = valid; + result_.success = valid_plan; result_.total_time = traj_time; server.setSucceeded(result_); state = PlannerState::WAITING; From 790d7d6eda6ae1d9ec2b34e70009cae08084812c Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sat, 21 Jul 2018 17:34:53 -0400 Subject: [PATCH 16/26] Fixed params; added vel and accel tolerances; misc fixes --- param/planner.yaml | 4 +- param/platform_2.0.yaml | 6 +- param/platform_crazyflie.yaml | 14 +++-- param/platform_sim.yaml | 6 +- src/MotionPlanner.cpp | 108 ++++++++++++++++++---------------- 5 files changed, 74 insertions(+), 64 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index cb8b5f9..a918000 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -4,7 +4,7 @@ transform_timeout : 0.2 # timeout for getting a successful plan -planning_timeout: .300 +planning_timeout: .150 # update frequency planner_update_frequency : 50.0 @@ -43,4 +43,4 @@ max_num: 1000 map_res: .25 # samples per sec for generating waypoints -samples_per_sec: 50 +samples_per_sec: 50.0 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 2c4a13b..a72a754 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -10,8 +10,10 @@ max_jerk: 15.0 # m/s^3 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 -# pose tolerances -p_tol: 0.05 +# tolerances +p_tol: 0.075 +vel_tol: 0.10 +accel_tol: 0.15 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index d55f790..770d040 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,17 +1,19 @@ # Params for drone size for planner -radius: 1.5 # meters -buffer: 0.2 # meters +radius: 0.05 # meters +buffer: 0.05 # meters # kinematic constraints -max_speed: 2.0 # m/s -max_acceleration: 3.0 # m/s^2 +max_speed: 1.5 # m/s +max_acceleration: 2.0 # m/s^2 max_jerk: 10.0 # m/s^3 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 -# pose tolerances -p_tol: 0.08 +# tolerances +p_tol: 0.075 +vel_tol: 0.10 +accel_tol: 0.15 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index 2c4a13b..a72a754 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -10,8 +10,10 @@ max_jerk: 15.0 # m/s^3 # Minimum height for any XY translationanl maneuvers, meters min_maneuver_height: 0.3 -# pose tolerances -p_tol: 0.05 +# tolerances +p_tol: 0.075 +vel_tol: 0.10 +accel_tol: 0.15 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index c26052b..6d142f9 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -113,123 +113,127 @@ int main(int argc, char **argv) { /******** LOAD PARAMETERS ********/ // node update frequency - double update_frequency = 50.0; + double update_frequency; // offset for coordinate frame differences with HLM - double coordinate_offset = 10.0; + double coordinate_offset; // max total planning time - double max_planning_time = .300; + double max_planning_time; // radius of drone in meters - double drone_radius = .625; + double drone_radius; // safety buffer to stay away from obstacles - double obst_buffer = .20; + double obst_buffer; /******** SETTINGS FOR PLANNER ********/ // min and max arena limits - double max_arena_limits[3] = {20, 20, 4}; - double min_arena_limits[3] = {0, 0, 0}; + double max_arena_limits[3]; + double min_arena_limits[3]; // speed, acceleration, jerk limits - double kinematic_constraints[3] = {3, 3, 15}; + double kinematic_constraints[3]; - // pose tolerances - double pose_tol = .05; + // tolerances + double pose_tol; + double vel_tol; + double accel_tol; // min and max time discretization - double min_dt = .05; - double max_dt = .15; + double min_dt; + double max_dt; // max time steps - int ndt = 1000; + int ndt; // greedy param, epsilon - double eps = 10; + double eps; // max for control space - double u_max = 15; + double u_max; // resolution to generate map at - double map_res = .25; + double map_res; // samples per sec for generating waypoints - double samples_per_sec = 50; + double samples_per_sec; // max num of expansions - int max_num = 5000; + int max_num; // control discretization - int num = 1; + int num; // controls which space to plan in - bool use_pos = true; - bool use_vel = true; - bool use_acc = true; - bool use_jrk = false; + bool use_pos; + bool use_vel; + bool use_acc; + bool use_jrk; // control space vec_Vec3f U; // Update frequency retrieve - ROS_ASSERT(private_nh.getParam("planner_update_frequency", update_frequency)); + private_nh.param("planner_update_frequency", update_frequency, 50.0); // max total planning time - ROS_ASSERT(private_nh.getParam("planning_timeout", max_planning_time)); + private_nh.param("planning_timeout", max_planning_time, 0.150); // used for obstacle dilation - ROS_ASSERT(private_nh.getParam("radius", drone_radius)); - ROS_ASSERT(private_nh.getParam("buffer", obst_buffer)); + private_nh.param("radius", drone_radius, 1.0); + private_nh.param("buffer", obst_buffer, 1.0); // max x, y, z arena limits - 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])); + 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 - 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])); + 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); // resolution to generate map at - ROS_ASSERT(private_nh.getParam("map_res", map_res)); + private_nh.param("map_res", map_res, .25); // speed, acceleration, jerk limits - 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])); + 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 - ROS_ASSERT(private_nh.getParam("min_dt", min_dt)); - ROS_ASSERT(private_nh.getParam("max_dt", max_dt)); + private_nh.param("min_dt", min_dt, 0.05); + private_nh.param("max_dt", max_dt, 0.15); - ROS_ASSERT(private_nh.getParam("use_pos", use_pos)); - ROS_ASSERT(private_nh.getParam("use_vel", use_vel)); - ROS_ASSERT(private_nh.getParam("use_acc", use_acc)); - ROS_ASSERT(private_nh.getParam("use_jrk", use_jrk)); + 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 - ROS_ASSERT(private_nh.getParam("ndt", ndt)); + private_nh.param("ndt", ndt, 1000); // Epsilon: greedy param - ROS_ASSERT(private_nh.getParam("eps", eps)); + private_nh.param("eps", eps, 10.0); // control discretization - ROS_ASSERT(private_nh.getParam("num", num)); + private_nh.param("num", num, 1); // maximum number of allowed expansion - ROS_ASSERT(private_nh.getParam("max_num", max_num)); + private_nh.param("max_num", max_num, 1000); - // position tolerances - ROS_ASSERT(private_nh.getParam("p_tol", pose_tol)); + // 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); // correction for obstacle-planner coordinate frame differences - ROS_ASSERT(private_nh.getParam("coordinate_offset", coordinate_offset)); + private_nh.param("coordinate_offset", coordinate_offset, 10.0); // number of samples per second to sample trajectory at - ROS_ASSERT(private_nh.getParam("samples_per_sec", samples_per_sec)); + private_nh.param("samples_per_sec", samples_per_sec, 50.0); // max jerk is u max u_max = kinematic_constraints[2]; @@ -473,7 +477,7 @@ int main(int argc, char **argv) { planner->setTmax(ndt * dt); planner->setMaxNum(max_num); planner->setU(U); - planner->setTol(pose_tol); + planner->setTol(pose_tol, vel_tol, accel_tol); // now we can plan ros::Time t0 = ros::Time::now(); From d87854abb989659ff578bcbf6c5c197830470a7e Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 22 Jul 2018 12:47:39 -0400 Subject: [PATCH 17/26] made obst buffer larger for crazyflie --- param/platform_crazyflie.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index 770d040..364f342 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner radius: 0.05 # meters -buffer: 0.05 # meters +buffer: 0.25 # meters # kinematic constraints max_speed: 1.5 # m/s From 6d8eeb77c9485b676f06017410453bed267ad03a Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 22 Jul 2018 18:19:37 -0400 Subject: [PATCH 18/26] fixes --- param/planner.yaml | 6 +++--- param/platform_crazyflie.yaml | 6 +++--- src/MotionPlanner.cpp | 13 +++++++++---- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index a918000..1053fb8 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -4,10 +4,10 @@ transform_timeout : 0.2 # timeout for getting a successful plan -planning_timeout: .150 +planning_timeout: .100 # update frequency -planner_update_frequency : 50.0 +planner_update_frequency : 60.0 # The flags use_xxx will adapt the planning in different control space # taken from motion primitive library README @@ -25,7 +25,7 @@ use_jrk: false # min/max time discretization for each primitive min_dt: 0.05 -max_dt: 0.15 +max_dt: 0.20 # max number of time discretization for a plan ndt: 1000 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index 364f342..a15279c 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,10 +1,10 @@ # Params for drone size for planner -radius: 0.05 # meters +radius: 0.25 # meters buffer: 0.25 # meters # kinematic constraints -max_speed: 1.5 # m/s -max_acceleration: 2.0 # m/s^2 +max_speed: 3.0 # m/s +max_acceleration: 3.0 # m/s^2 max_jerk: 10.0 # m/s^3 # Minimum height for any XY translationanl maneuvers, meters diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 6d142f9..f6ddf76 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -456,7 +456,6 @@ int main(int argc, char **argv) { goal.use_acc = use_acc; goal.use_jrk = use_jrk; - double dt = max_dt; bool done_planning = false; bool valid_plan = false; @@ -465,6 +464,9 @@ int main(int argc, char **argv) { 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)); @@ -474,7 +476,7 @@ int main(int argc, char **argv) { planner->setAmax(kinematic_constraints[1]); planner->setUmax(kinematic_constraints[2]); planner->setDt(dt); - planner->setTmax(ndt * dt); + planner->setTmax(ndt*dt); planner->setMaxNum(max_num); planner->setU(U); planner->setTol(pose_tol, vel_tol, accel_tol); @@ -487,10 +489,13 @@ int main(int argc, char **argv) { if (valid) { traj = planner->getTraj(); valid_plan = true; + done_planning = ((ros::Time::now() + planning_time) < time_done); + } else { + done_planning = true; } - done_planning = (done_planning && ((ros::Time::now() + planning_time) < time_done)); - dt = dt - .05; + i++; + dt = max_dt-i*.05; } if (!valid_plan) { From 2f2ae9a6da64f7400a2c2d01e2fb17870674cfdd Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Tue, 24 Jul 2018 22:23:09 -0400 Subject: [PATCH 19/26] support start offset --- param/planner.yaml | 7 +- param/platform_2.0.yaml | 19 +- param/platform_crazyflie.yaml | 19 +- param/platform_sim.yaml | 19 +- src/MotionPlanner.cpp | 394 ++++++++++++++++------------------ 5 files changed, 211 insertions(+), 247 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index 1053fb8..cabaca8 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -1,8 +1,5 @@ # PARAMS FOR GRAPH SEARCH PLANNER -# Timeout for transforms -transform_timeout : 0.2 - # timeout for getting a successful plan planning_timeout: .100 @@ -24,7 +21,7 @@ use_acc: true use_jrk: false # min/max time discretization for each primitive -min_dt: 0.05 +min_dt: 0.10 max_dt: 0.20 # max number of time discretization for a plan @@ -37,7 +34,7 @@ eps: 10.0 num: 1 # maximum number of allowed expansion; -1 means no limit -max_num: 1000 +max_num: 1500 # resolution to generate map at map_res: .25 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index a72a754..e70a0d3 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,19 +1,16 @@ # Params for drone size for planner -radius: 0.625 # meters -buffer: 0.2 # meters +radius: 0.7 # meters +buffer: 0.5 # meters # kinematic constraints -max_speed: 3.0 # m/s -max_acceleration: 3.0 # m/s^2 -max_jerk: 15.0 # m/s^3 - -# Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +max_speed: 0.75 # m/s +max_acceleration: 1.5 # m/s^2 +max_jerk: 7.5 # m/s^3 # tolerances -p_tol: 0.075 -vel_tol: 0.10 -accel_tol: 0.15 +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index a15279c..e70a0d3 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,19 +1,16 @@ # Params for drone size for planner -radius: 0.25 # meters -buffer: 0.25 # meters +radius: 0.7 # meters +buffer: 0.5 # meters # kinematic constraints -max_speed: 3.0 # m/s -max_acceleration: 3.0 # m/s^2 -max_jerk: 10.0 # m/s^3 - -# Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +max_speed: 0.75 # m/s +max_acceleration: 1.5 # m/s^2 +max_jerk: 7.5 # m/s^3 # tolerances -p_tol: 0.075 -vel_tol: 0.10 -accel_tol: 0.15 +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index a72a754..e70a0d3 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -1,19 +1,16 @@ # Params for drone size for planner -radius: 0.625 # meters -buffer: 0.2 # meters +radius: 0.7 # meters +buffer: 0.5 # meters # kinematic constraints -max_speed: 3.0 # m/s -max_acceleration: 3.0 # m/s^2 -max_jerk: 15.0 # m/s^3 - -# Minimum height for any XY translationanl maneuvers, meters -min_maneuver_height: 0.3 +max_speed: 0.75 # m/s +max_acceleration: 1.5 # m/s^2 +max_jerk: 7.5 # m/s^3 # tolerances -p_tol: 0.075 -vel_tol: 0.10 -accel_tol: 0.15 +p_tol: 0.1 +vel_tol: 0.15 +accel_tol: 0.25 # offset to account for coordinate frame differences coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index f6ddf76..c4521c0 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -49,8 +49,6 @@ using geometry_msgs::Point; typedef actionlib::SimpleActionServer Server; -enum class PlannerState { WAITING, PLANNING }; - bool checkGoal(Point &pose_goal, Vector3 &twist_goal, Vector3 &accel_goal, double kinematic_constraints[3], double max_arena[3], @@ -256,10 +254,6 @@ int main(int argc, char **argv) { Vector3 twist_goal, twist_start; Vector3 accel_goal, accel_start; - // for caching maps - bool new_obstacle_available = false; - planning_ros_msgs::VoxelMap last_map; - // map util std::shared_ptr map_util(new MPL::VoxelMapUtil); @@ -269,8 +263,6 @@ int main(int argc, char **argv) { [&](const boost::shared_ptr& msg) -> void { if (last_msg == nullptr || last_msg->header.stamp < msg->header.stamp) { last_msg = msg; - new_obstacle_available = true; - } else { ROS_ERROR("Motion Planner: bad stamp on obstacle message"); } @@ -288,8 +280,6 @@ int main(int argc, char **argv) { // node update rate ros::Rate rate(update_frequency); - PlannerState state = PlannerState::WAITING; - // Run until ROS says we need to shutdown while (ros::ok()) { // Check the safety client before updating anything @@ -299,18 +289,14 @@ int main(int argc, char **argv) { ROS_ASSERT_MSG(!safety_client.isSafetyActive(), "MotionPlanner shutdown due to safety active"); // goal was canceled - if (server.isPreemptRequested() && - state == PlannerState::PLANNING) { + if (server.isPreemptRequested()) { server.setPreempted(); ROS_INFO("Preempt requested. Current planning goal was canceled"); - state = PlannerState::WAITING; } if (last_msg == nullptr) { ROS_ERROR("MotionPlanner: No obstacle information available, can't build map"); - new_obstacle_available = false; - state = PlannerState::WAITING; - } else if (state == PlannerState::WAITING) { + } else { // get a new goal from action server if (server.isNewGoalAvailable()) { if (server.isActive()) { @@ -319,224 +305,214 @@ int main(int argc, char **argv) { // planning goal goal_ = server.acceptNewGoal(); - pose_goal = goal_->goal.motion_point.pose.position; - twist_goal = goal_->goal.motion_point.twist.linear; - accel_goal = goal_->goal.motion_point.accel.linear; + // offset by coordinate frame minus start pose so that we never "go out" of arena + double x_offset = coordinate_offset - goal_->start.motion_point.pose.position.x; + double y_offset = coordinate_offset - goal_->start.motion_point.pose.position.y; + + 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; - pose_start = goal_->start.motion_point.pose.position; twist_start = goal_->start.motion_point.twist.linear; accel_start = goal_->start.motion_point.accel.linear; + 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; + + twist_goal = goal_->goal.motion_point.twist.linear; + accel_goal = goal_->goal.motion_point.accel.linear; + if (!checkGoal(pose_goal, twist_goal, accel_goal, kinematic_constraints, max_arena_limits, min_arena_limits)) { ROS_ERROR("Planner aborting requested goal"); - server.setAborted(); - state = PlannerState::WAITING; - } else { - ROS_DEBUG_THROTTLE(30,"New goal accepted by planner, now map"); - } - - state = PlannerState::PLANNING; - - } else { - ROS_DEBUG_THROTTLE(30, "Planner: No new goal. Generate map only"); - state = PlannerState::WAITING; - } - if (new_obstacle_available) { - ros::WallTime t1 = ros::WallTime::now(); - - // Standard header - std_msgs::Header header; - header.frame_id = std::string("map"); - - 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; - - sensor_msgs::PointCloud cloud; - - // Generate cloud from obstacle data - cloud.header.stamp = ros::Time::now(); - cloud.header.frame_id = "cloud"; - cloud.channels.resize(1); - - std::array voxel_map_origin = - {map.origin.x, map.origin.y, map.origin.z}; - - for (auto& obstacle : last_msg->obstacles) { - // Map each obstacle to the cloud - float pipe_radius = obstacle.pipe_radius; - float pipe_height = obstacle.pipe_height; - float pipe_x = (obstacle.odom.pose.pose.position.x + - coordinate_offset); - float pipe_y = (obstacle.odom.pose.pose.position.y + - coordinate_offset); - float px, py, pz; - for (pz = voxel_map_origin[2]; pz <= pipe_height; pz += 0.1) { - for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = 0; r < (pipe_radius + buffer); 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]; - geometry_msgs::Point32 point; - point.x = px; - point.y = py; - point.z = std::min(max_arena_limits[2], pz + buffer); - cloud.points.push_back(point); + 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(); + + // Standard header + std_msgs::Header header; + header.frame_id = std::string("map"); + + 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; + + sensor_msgs::PointCloud cloud; + + // Generate cloud from obstacle data + cloud.header.stamp = ros::Time::now(); + cloud.header.frame_id = "cloud"; + cloud.channels.resize(1); + + std::array voxel_map_origin = + {map.origin.x, map.origin.y, map.origin.z}; + + for (auto& obstacle : last_msg->obstacles) { + // Map each obstacle to the cloud + float pipe_radius = obstacle.pipe_radius; + float pipe_height = obstacle.pipe_height; + 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 <= pipe_height; pz += 0.1) { + for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { + for (float r = 0; r < (pipe_radius + buffer); 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]; + if (!(px<0.0 || py<0.0)) { + geometry_msgs::Point32 point; + point.x = px; + point.y = py; + point.z = std::min(max_arena_limits[2], pz + buffer); + cloud.points.push_back(point); + } + } } } } - } - - vec_Vec3f pts = cloud_to_vec(cloud); - std::array voxel_map_dim = - {map.dim.x * map_res, - map.dim.y * map_res, - map.dim.z * map_res}; - - std::unique_ptr voxel_grid( - new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); - - voxel_grid->addCloud(pts); - voxel_grid->getMap(map); + vec_Vec3f pts = cloud_to_vec(cloud); + + std::array voxel_map_dim = + {map.dim.x * map_res, + map.dim.y * map_res, + map.dim.z * map_res}; + + std::unique_ptr voxel_grid( + new VoxelGrid(voxel_map_origin, voxel_map_dim, map_res)); + + voxel_grid->addCloud(pts); + voxel_grid->getMap(map); + + map.header = cloud.header; + + // 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->setMap(ori, dim, map.data, map_res); + + ROS_INFO("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; + } - map.header = cloud.header; + i++; + dt = max_dt-i*.05; + } - // 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); + 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; - map_util->setMap(ori, dim, map.data, map_res); + vec_E> waypoints = traj.sample(n_samples); - ROS_DEBUG_THROTTLE(30, "Takes %f sec for building map", - (ros::WallTime::now() - t1).toSec()); + ros::Time time_point = goal_->header.stamp; + ros::Duration t_step = ros::Duration(traj_time/waypoints.size()); - last_map = map; + for (Waypoint<3> waypoint : waypoints) { + iarc7_msgs::MotionPointStamped m_point; + m_point.header.frame_id = "map"; + m_point.header.stamp = time_point; - new_obstacle_available = false; - } else if (last_msg != nullptr) { - ROS_DEBUG_THROTTLE(30, "MotionPlanner: No new obstacle info available. Using previously generated map"); + 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); - Vec3f ori(last_map.origin.x, last_map.origin.y, last_map.origin.z); - Vec3i dim(last_map.dim.x, last_map.dim.y, last_map.dim.z); + 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); - map_util->setMap(ori, dim, last_map.data, map_res); - } - } + 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); - // able to generate a plan - if (state == PlannerState::PLANNING) { - 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; + 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_); + } } - - i++; - dt = max_dt-i*.05; - } - - if (!valid_plan) { - ROS_ERROR("Planner failed to find any plan!"); - result_.success = valid_plan; - server.setSucceeded(result_); - state = PlannerState::WAITING; } 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)-coordinate_offset; - m_point.motion_point.pose.position.y = waypoint.pos(1)-coordinate_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_); - state = PlannerState::WAITING; + ROS_DEBUG_THROTTLE(30, "Planner: No new goal."); } } // Handle all ROS callbacks From 017f267fe169fb559e1f2f0792a33182fb902416 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 26 Jul 2018 22:29:40 -0400 Subject: [PATCH 20/26] fixes and clean up --- param/platform_2.0.yaml | 7 ++--- param/platform_crazyflie.yaml | 7 ++--- param/platform_sim.yaml | 7 ++--- src/MotionPlanner.cpp | 52 ++++++++++------------------------- 4 files changed, 20 insertions(+), 53 deletions(-) diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index e70a0d3..5d4bf84 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner -radius: 0.7 # meters -buffer: 0.5 # meters +radius: 0.675 # meters +buffer: 0.325 # meters # kinematic constraints max_speed: 0.75 # m/s @@ -11,6 +11,3 @@ max_jerk: 7.5 # m/s^3 p_tol: 0.1 vel_tol: 0.15 accel_tol: 0.25 - -# offset to account for coordinate frame differences -coordinate_offset: 10 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index e70a0d3..5d4bf84 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner -radius: 0.7 # meters -buffer: 0.5 # meters +radius: 0.675 # meters +buffer: 0.325 # meters # kinematic constraints max_speed: 0.75 # m/s @@ -11,6 +11,3 @@ max_jerk: 7.5 # m/s^3 p_tol: 0.1 vel_tol: 0.15 accel_tol: 0.25 - -# offset to account for coordinate frame differences -coordinate_offset: 10 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index e70a0d3..5d4bf84 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner -radius: 0.7 # meters -buffer: 0.5 # meters +radius: 0.675 # meters +buffer: 0.325 # meters # kinematic constraints max_speed: 0.75 # m/s @@ -11,6 +11,3 @@ max_jerk: 7.5 # m/s^3 p_tol: 0.1 vel_tol: 0.15 accel_tol: 0.25 - -# offset to account for coordinate frame differences -coordinate_offset: 10 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index c4521c0..1a68ad2 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -113,9 +113,6 @@ int main(int argc, char **argv) { // node update frequency double update_frequency; - // offset for coordinate frame differences with HLM - double coordinate_offset; - // max total planning time double max_planning_time; @@ -227,9 +224,6 @@ int main(int argc, char **argv) { private_nh.param("vel_tol", vel_tol, .1); private_nh.param("accel_tol", accel_tol, .15); - // correction for obstacle-planner coordinate frame differences - private_nh.param("coordinate_offset", coordinate_offset, 10.0); - // number of samples per second to sample trajectory at private_nh.param("samples_per_sec", samples_per_sec, 50.0); @@ -254,9 +248,6 @@ int main(int argc, char **argv) { Vector3 twist_goal, twist_start; Vector3 accel_goal, accel_start; - // map util - std::shared_ptr map_util(new MPL::VoxelMapUtil); - // subscribe to obstacle topic boost::shared_ptr last_msg; boost::function&)> obstacle_callback = @@ -305,12 +296,12 @@ int main(int argc, char **argv) { // planning goal goal_ = server.acceptNewGoal(); - // offset by coordinate frame minus start pose so that we never "go out" of arena - double x_offset = coordinate_offset - goal_->start.motion_point.pose.position.x; - double y_offset = coordinate_offset - goal_->start.motion_point.pose.position.y; + // offset by start pose so that we never "go out" of arena + double x_offset = -goal_->start.motion_point.pose.position.x; + double y_offset = -goal_->start.motion_point.pose.position.y; - 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.x = 0.0; + pose_start.y = 0.0; pose_start.z = goal_->start.motion_point.pose.position.z; twist_start = goal_->start.motion_point.twist.linear; @@ -337,10 +328,6 @@ int main(int argc, char **argv) { ros::WallTime t1 = ros::WallTime::now(); - // Standard header - std_msgs::Header header; - header.frame_id = std::string("map"); - planning_ros_msgs::VoxelMap map; map.resolution = map_res; map.origin.x = min_arena_limits[0]/map_res; @@ -352,42 +339,30 @@ int main(int argc, char **argv) { std::vector data(map.dim.x * map.dim.y * map.dim.z, 0); map.data = data; - sensor_msgs::PointCloud cloud; - - // Generate cloud from obstacle data - cloud.header.stamp = ros::Time::now(); - cloud.header.frame_id = "cloud"; - cloud.channels.resize(1); - 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; + float pipe_radius = obstacle.pipe_radius + buffer; float pipe_height = obstacle.pipe_height; 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 <= pipe_height; pz += 0.1) { for (float theta = 0; theta < 2 * M_PI; theta += 0.15) { - for (float r = 0; r < (pipe_radius + buffer); r += map_res) { + 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]; - if (!(px<0.0 || py<0.0)) { - geometry_msgs::Point32 point; - point.x = px; - point.y = py; - point.z = std::min(max_arena_limits[2], pz + buffer); - cloud.points.push_back(point); - } + Vec3f pt(px, py, std::min(max_arena_limits[2], pz + buffer)); + pts.push_back(pt); } } } } - vec_Vec3f pts = cloud_to_vec(cloud); - std::array voxel_map_dim = {map.dim.x * map_res, map.dim.y * map_res, @@ -399,12 +374,13 @@ int main(int argc, char **argv) { voxel_grid->addCloud(pts); voxel_grid->getMap(map); - map.header = cloud.header; - // 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_INFO("Takes %f sec for building and setting map", From 3e15d70e3a816450e73ffe74e09f0809de7f6e88 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Fri, 27 Jul 2018 20:22:31 -0400 Subject: [PATCH 21/26] fixes --- param/planner.yaml | 2 +- param/platform_2.0.yaml | 8 ++++---- param/platform_crazyflie.yaml | 8 ++++---- param/platform_sim.yaml | 8 ++++---- src/MotionPlanner.cpp | 20 ++++++++++---------- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/param/planner.yaml b/param/planner.yaml index cabaca8..5556a47 100644 --- a/param/planner.yaml +++ b/param/planner.yaml @@ -22,7 +22,7 @@ use_jrk: false # min/max time discretization for each primitive min_dt: 0.10 -max_dt: 0.20 +max_dt: 0.25 # max number of time discretization for a plan ndt: 1000 diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 5d4bf84..5ec3d12 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,11 +1,11 @@ # Params for drone size for planner -radius: 0.675 # meters -buffer: 0.325 # meters +radius: 0.6 # meters +buffer: 0.1 # meters # kinematic constraints max_speed: 0.75 # m/s -max_acceleration: 1.5 # m/s^2 -max_jerk: 7.5 # m/s^3 +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 # tolerances p_tol: 0.1 diff --git a/param/platform_crazyflie.yaml b/param/platform_crazyflie.yaml index 5d4bf84..5ec3d12 100644 --- a/param/platform_crazyflie.yaml +++ b/param/platform_crazyflie.yaml @@ -1,11 +1,11 @@ # Params for drone size for planner -radius: 0.675 # meters -buffer: 0.325 # meters +radius: 0.6 # meters +buffer: 0.1 # meters # kinematic constraints max_speed: 0.75 # m/s -max_acceleration: 1.5 # m/s^2 -max_jerk: 7.5 # m/s^3 +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 # tolerances p_tol: 0.1 diff --git a/param/platform_sim.yaml b/param/platform_sim.yaml index 5d4bf84..5ec3d12 100644 --- a/param/platform_sim.yaml +++ b/param/platform_sim.yaml @@ -1,11 +1,11 @@ # Params for drone size for planner -radius: 0.675 # meters -buffer: 0.325 # meters +radius: 0.6 # meters +buffer: 0.1 # meters # kinematic constraints max_speed: 0.75 # m/s -max_acceleration: 1.5 # m/s^2 -max_jerk: 7.5 # m/s^3 +max_acceleration: 1.0 # m/s^2 +max_jerk: 4.0 # m/s^3 # tolerances p_tol: 0.1 diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 1a68ad2..c908509 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -286,7 +286,7 @@ int main(int argc, char **argv) { } if (last_msg == nullptr) { - ROS_ERROR("MotionPlanner: No obstacle information available, can't build map"); + ROS_DEBUG_THROTTLE(30,"MotionPlanner: No obstacle information available, can't build map"); } else { // get a new goal from action server if (server.isNewGoalAvailable()) { @@ -297,11 +297,11 @@ int main(int argc, char **argv) { goal_ = server.acceptNewGoal(); // offset by start pose so that we never "go out" of arena - double x_offset = -goal_->start.motion_point.pose.position.x; - double y_offset = -goal_->start.motion_point.pose.position.y; + double x_offset = 10; + double y_offset = 10; - pose_start.x = 0.0; - pose_start.y = 0.0; + 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; @@ -356,7 +356,7 @@ int main(int argc, char **argv) { 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, std::min(max_arena_limits[2], pz + buffer)); + Vec3f pt(px, py, pz); pts.push_back(pt); } } @@ -364,9 +364,9 @@ int main(int argc, char **argv) { } std::array voxel_map_dim = - {map.dim.x * map_res, - map.dim.y * map_res, - map.dim.z * map_res}; + {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)); @@ -383,7 +383,7 @@ int main(int argc, char **argv) { map_util->setMap(ori, dim, map.data, map_res); - ROS_INFO("Takes %f sec for building and setting map", + ROS_DEBUG_THROTTLE(30,"Takes %f sec for building and setting map", (ros::WallTime::now() - t1).toSec()); iarc7_msgs::PlanResult result_; From 18b956de91b06413a72ad144ac8b7f34247c6e7f Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 29 Jul 2018 14:23:59 -0400 Subject: [PATCH 22/26] added back in start offset --- src/MotionPlanner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index c908509..e8ce3c2 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -297,8 +297,8 @@ int main(int argc, char **argv) { goal_ = server.acceptNewGoal(); // offset by start pose so that we never "go out" of arena - double x_offset = 10; - double y_offset = 10; + double x_offset = 10-goal_->start.motion_point.pose.position.x; + double y_offset = 10-goal_->start.motion_point.pose.position.y; pose_start.x = goal_->start.motion_point.pose.position.x + x_offset; pose_start.y = goal_->start.motion_point.pose.position.y + y_offset; From 1b562594d3e6140d3c702f64bebca17a735134ff Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 29 Jul 2018 15:27:52 -0400 Subject: [PATCH 23/26] param change --- param/platform_2.0.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 5ec3d12..8543a26 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner radius: 0.6 # meters -buffer: 0.1 # meters +buffer: 1.5 # meters # kinematic constraints max_speed: 0.75 # m/s From f153ac89428ee01b4f7c11d5a768831f531bddaa Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 29 Jul 2018 15:35:40 -0400 Subject: [PATCH 24/26] fixed offsets --- src/MotionPlanner.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index e8ce3c2..087b6fb 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -297,8 +297,8 @@ int main(int argc, char **argv) { goal_ = server.acceptNewGoal(); // offset by start pose so that we never "go out" of arena - double x_offset = 10-goal_->start.motion_point.pose.position.x; - double y_offset = 10-goal_->start.motion_point.pose.position.y; + 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; pose_start.x = goal_->start.motion_point.pose.position.x + x_offset; pose_start.y = goal_->start.motion_point.pose.position.y + y_offset; From 579038d5207fa1138a6ee176ceccc75d12a23b74 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Sun, 29 Jul 2018 16:25:05 -0400 Subject: [PATCH 25/26] fixed obst buffer --- src/MotionPlanner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/MotionPlanner.cpp b/src/MotionPlanner.cpp index 087b6fb..aca9730 100644 --- a/src/MotionPlanner.cpp +++ b/src/MotionPlanner.cpp @@ -347,11 +347,10 @@ int main(int argc, char **argv) { for (auto& obstacle : last_msg->obstacles) { // Map each obstacle to the cloud float pipe_radius = obstacle.pipe_radius + buffer; - float pipe_height = obstacle.pipe_height; 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 <= pipe_height; pz += 0.1) { + 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]; From db607f75b914c151c68e2e955572ee83dc184cb4 Mon Sep 17 00:00:00 2001 From: Pitt RAS IARC Team Date: Sun, 5 Aug 2018 15:49:01 -0400 Subject: [PATCH 26/26] Changes on drone --- param/platform_2.0.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/param/platform_2.0.yaml b/param/platform_2.0.yaml index 8543a26..5d8edb4 100644 --- a/param/platform_2.0.yaml +++ b/param/platform_2.0.yaml @@ -1,6 +1,6 @@ # Params for drone size for planner radius: 0.6 # meters -buffer: 1.5 # meters +buffer: 2.0 # meters # kinematic constraints max_speed: 0.75 # m/s