diff --git a/Makefile b/Makefile
deleted file mode 100755
index b75b928f20ef9ea4f509a17db62e4e31b422c27f..0000000000000000000000000000000000000000
--- a/Makefile
+++ /dev/null
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
diff --git a/cfg/ackermann_local_planner_alg_config.cfg b/cfg/ackermann_local_planner_alg_config.cfg
deleted file mode 100755
index 8f2246fd451eaec706d72181495fddcc46f07847..0000000000000000000000000000000000000000
--- a/cfg/ackermann_local_planner_alg_config.cfg
+++ /dev/null
@@ -1,76 +0,0 @@
-#! /usr/bin/env python
-#* All rights reserved.
-#*
-#* Redistribution and use in source and binary forms, with or without
-#* modification, are permitted provided that the following conditions
-#* are met:
-#*
-#* * Redistributions of source code must retain the above copyright
-#* notice, this list of conditions and the following disclaimer.
-#* * Redistributions in binary form must reproduce the above
-#* copyright notice, this list of conditions and the following
-#* disclaimer in the documentation and/or other materials provided
-#* with the distribution.
-#* * Neither the name of the Willow Garage nor the names of its
-#* contributors may be used to endorse or promote products derived
-#* from this software without specific prior written permission.
-#*
-#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-#* POSSIBILITY OF SUCH DAMAGE.
-#***********************************************************
-
-# Author:
-
-PACKAGE='iri_ackermann_local_planner'
-import roslib; roslib.load_manifest(PACKAGE)
-
-import sys
-from math import pi
-
-from dynamic_reconfigure.parameter_generator import *
-
-gen = ParameterGenerator()
-
-# Name Type Reconfiguration level Description Default Min Max
-gen.add("ack_vel_max", double_t, 0, "maximum translational speed", 0.3, 0, 20)
-gen.add("ack_vel_min", double_t, 0, "minimum translational speed", -0.3, -20, 0)
-gen.add("ack_acc_max", double_t, 0, "maximum translational acceleration", 1.0, 0, 20)
-gen.add("ack_steer_angle_max", double_t, 0, "maximum steer angle", 0.45, 0, 0.54)
-gen.add("ack_steer_angle_min", double_t, 0, "minimum steer angle", -0.45, -0.54, 0)
-gen.add("ack_steer_speed_max", double_t, 0, "maximum steer speed", 1.0, 0, 2)
-gen.add("ack_steer_speed_min", double_t, 0, "minimum steer speed", -1.0, -2, 0)
-gen.add("ack_steer_acc_max", double_t, 0, "maximum steer acceleration", 0.36, 0, 5)
-gen.add("ack_axis_distance", double_t, 0, "distance between axes", 1.65, 0, 2)
-
-gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 10)
-gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
-
-gen.add("pdist_scale", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0, 5)
-gen.add("gdist_scale", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0, 5)
-gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01,0, 5)
-gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0 ,0, 500)
-gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64)
-
-gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 20, 1, 300)
-gen.add("vtheta_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1, 300)
-
-gen.add("simple_attractor", bool_t, 0, "Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation", False)
-
-gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2)
-
-gen.add("xy_goal_tolerance", double_t, 0, "Minimum distance to end the current goal", 0.5, 0, 1)
-gen.add("heading_goal_tolerance", double_t, 0, "Minimum angle to end the current goal", 0.1, 0, pi/2)
-
-gen.add("restore_defaults", bool_t, 0, "Retore to the default configuration", False)
-
-exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner"))
diff --git a/include/costmap_model.h b/include/costmap_model.h
deleted file mode 100755
index d3ac57945860ce9f0405e15ba76a472291ca5bd8..0000000000000000000000000000000000000000
--- a/include/costmap_model.h
+++ /dev/null
@@ -1,98 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
-#define TRAJECTORY_ROLLOUT_COSTMAP_MODEL_
-
-#include <world_model.h>
-// For obstacle data access
-#include <costmap_2d/costmap_2d.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class CostmapModel
- * @brief A class that implements the WorldModel interface to provide grid
- * based collision checks for the trajectory controller using the costmap.
- */
- class CostmapModel : public WorldModel {
- public:
- /**
- * @brief Constructor for the CostmapModel
- * @param costmap The costmap that should be used
- * @return
- */
- CostmapModel(const costmap_2d::Costmap2D& costmap);
-
- /**
- * @brief Destructor for the world model
- */
- virtual ~CostmapModel(){}
-
- /**
- * @brief Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid
- * @param position The position of the robot in world coordinates
- * @param footprint The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @return Positive if all the points lie outside the footprint, negative otherwise
- */
- virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius);
-
- private:
- /**
- * @brief Rasterizes a line in the costmap grid and checks for collisions
- * @param x0 The x position of the first cell in grid coordinates
- * @param y0 The y position of the first cell in grid coordinates
- * @param x1 The x position of the second cell in grid coordinates
- * @param y1 The y position of the second cell in grid coordinates
- * @return A positive cost for a legal line... negative otherwise
- */
- double lineCost(int x0, int x1, int y0, int y1);
-
- /**
- * @brief Checks the cost of a point in the costmap
- * @param x The x position of the point in cell coordinates
- * @param y The y position of the point in cell coordinates
- * @return A positive cost for a legal point... negative otherwise
- */
- double pointCost(int x, int y);
-
- const costmap_2d::Costmap2D& costmap_; ///< @brief Allows access of costmap obstacle information
-
- };
-};
-#endif
diff --git a/include/goal_functions.h b/include/goal_functions.h
deleted file mode 100755
index 03262cdc2bbc60db493b0e6f736dd1dc8995340e..0000000000000000000000000000000000000000
--- a/include/goal_functions.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2009, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
-#define BASE_LOCAL_PLANNER_GOAL_FUNCTIONS_H_
-
-#include <ros/ros.h>
-#include <tf/transform_listener.h>
-#include <tf/transform_datatypes.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/Path.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/Point.h>
-#include <tf/transform_listener.h>
-
-#include <string>
-#include <cmath>
-
-#include <angles/angles.h>
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/costmap_2d_ros.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @brief Compute the distance between two points
- * @param x1 The first x point
- * @param y1 The first y point
- * @param x2 The second x point
- * @param y2 The second y point
- */
- double distance(double x1, double y1, double x2, double y2);
-
- /**
- * @brief Check if the goal position has been achieved
- * @param global_pose The pose of the robot in the global frame
- * @param goal_x The desired x value for the goal
- * @param goal_y The desired y value for the goal
- * @param xy_goal_tolerance The tolerance on the position
- * @return True if achieved, false otherwise
- */
- bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y, double xy_goal_tolerance);
-
- /**
- * @brief Check if the goal orientation has been achieved
- * @param global_pose The pose of the robot in the global frame
- * @param goal_x The desired x value for the goal
- * @param goal_y The desired y value for the goal
- * @param yaw_goal_tolerance The tolerance on the position
- * @return True if achieved, false otherwise
- */
- bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance);
-
- /**
- * @brief Publish a plan for visualization purposes
- * @param path The plan to publish
- * @param pub The published to use
- * @param r,g,b,a The color and alpha value to use when publishing
- */
- void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a);
-
- /**
- * @brief Trim off parts of the global plan that are far enough behind the robot
- * @param global_pose The pose of the robot in the global frame
- * @param plan The plan to be pruned
- * @param global_plan The plan to be pruned in the frame of the planner
- */
- void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan);
-
- /**
- * @brief Transforms the global plan of the robot from the planner frame to the local frame
- * @param tf A reference to a transform listener
- * @param global_plan The plan to be transformed
- * @param costmap A reference to the costmap being used so the window size for transforming can be computed
- * @param global_frame The frame to transform the plan to
- * @param transformed_plan Populated with the transformed plan
- */
- bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
- const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
- std::vector<geometry_msgs::PoseStamped>& transformed_plan);
-
- /**
- * @brief Check if the goal pose has been achieved
- * @param tf A reference to a transform listener
- * @param global_plan The plan being followed
- * @param costmap_ros A reference to the costmap object being used by the planner
- * @param global_frame The global frame of the local planner
- * @param base_odom The current odometry information for the robot
- * @param rot_stopped_vel The rotational velocity below which the robot is considered stopped
- * @param trans_stopped_vel The translational velocity below which the robot is considered stopped
- * @param xy_goal_tolerance The translational tolerance on reaching the goal
- * @param yaw_goal_tolerance The rotational tolerance on reaching the goal
- * @return True if achieved, false otherwise
- */
- bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
- const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
- const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
- double xy_goal_tolerance, double yaw_goal_tolerance);
-
- /**
- * @brief Check whether the robot is stopped or not
- * @param base_odom The current odometry information for the robot
- * @param rot_stopped_velocity The rotational velocity below which the robot is considered stopped
- * @param trans_stopped_velocity The translational velocity below which the robot is considered stopped
- * @return True if the robot is stopped, false otherwise
- */
- bool stopped(const nav_msgs::Odometry& base_odom,
- const double& rot_stopped_velocity, const double& trans_stopped_velocity);
-};
-#endif
diff --git a/include/map_cell.h b/include/map_cell.h
deleted file mode 100755
index 34c03e0525a104e5864b8e7d149bf43c636722d9..0000000000000000000000000000000000000000
--- a/include/map_cell.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_MAP_CELL_H_
-#define TRAJECTORY_ROLLOUT_MAP_CELL_H_
-
-#include <trajectory_inc.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class MapCell
- * @brief Stores path distance and goal distance information used for scoring trajectories
- */
- class MapCell{
- public:
- /**
- * @brief Default constructor
- */
- MapCell();
-
- /**
- * @brief Copy constructor
- * @param mc The MapCell to be copied
- */
- MapCell(const MapCell& mc);
-
- unsigned int cx, cy; ///< @brief Cell index in the grid map
-
- double path_dist; ///< @brief Distance to planner's path
-
- double goal_dist; ///< @brief Distance to local goal
-
- double occ_dist; ///< @brief Distance to obstacles
-
- int occ_state; ///< @brief Occupancy state (-1 = free, 0 = unknown, 1 = occupied)
-
- bool path_mark, goal_mark; ///< @brief Marks fir computing path/goal distances
-
- bool within_robot; ///< @brief Mark for cells within the robot footprint
- };
-};
-
-#endif
diff --git a/include/map_grid.h b/include/map_grid.h
deleted file mode 100755
index 9a912186c68f1e2751fb466230b0020319f61a0c..0000000000000000000000000000000000000000
--- a/include/map_grid.h
+++ /dev/null
@@ -1,220 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_MAP_GRID_H_
-#define TRAJECTORY_ROLLOUT_MAP_GRID_H_
-
-#include <vector>
-#include <iostream>
-#include <trajectory_inc.h>
-#include <ros/console.h>
-#include <ros/ros.h>
-
-#include <map_cell.h>
-#include <costmap_2d/costmap_2d.h>
-#include <geometry_msgs/PoseStamped.h>
-
-namespace iri_ackermann_local_planner{
- /**
- * @class MapGrid
- * @brief A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controller.
- */
- class MapGrid{
- public:
- /**
- * @brief Creates a 0x0 map by default
- */
- MapGrid();
-
- /**
- * @brief Creates a map of size_x by size_y
- * @param size_x The width of the map
- * @param size_y The height of the map
- */
- MapGrid(unsigned int size_x, unsigned int size_y);
-
- /**
- * @brief Creates a map of size_x by size_y with the desired scale and origin
- * @param size_x The width of the map
- * @param size_y The height of the map
- * @param scale The resolution of each MapCell
- * @param x The x coordinate of the origin of the map
- * @param y The y coordinate of the origin of the map
- */
- MapGrid(unsigned int size_x, unsigned int size_y, double scale, double x, double y);
-
- /**
- * @brief Returns a map cell accessed by (col, row)
- * @param x The x coordinate of the cell
- * @param y The y coordinate of the cell
- * @return A reference to the desired cell
- */
- inline MapCell& operator() (unsigned int x, unsigned int y){
- return map_[size_x_ * y + x];
- }
-
- /**
- * @brief Returns a map cell accessed by (col, row)
- * @param x The x coordinate of the cell
- * @param y The y coordinate of the cell
- * @return A copy of the desired cell
- */
- inline MapCell operator() (unsigned int x, unsigned int y) const {
- return map_[size_x_ * y + x];
- }
-
- inline MapCell& getCell(unsigned int x, unsigned int y){
- return map_[size_x_ * y + x];
- }
-
- /**
- * @brief Destructor for a MapGrid
- */
- ~MapGrid(){}
-
- /**
- * @brief Copy constructor for a MapGrid
- * @param mg The MapGrid to copy
- */
- MapGrid(const MapGrid& mg);
-
- /**
- * @brief Assignment operator for a MapGrid
- * @param mg The MapGrid to assign from
- */
- MapGrid& operator= (const MapGrid& mg);
-
- /**
- * @brief reset path distance fields for all cells
- */
- void resetPathDist();
-
- /**
- * @brief check if we need to resize
- * @param size_x The desired width
- * @param size_y The desired height
- * @param o_x the desired x coordinate of the origin
- * @param o_y the desired y coordinate of the origin
- */
- void sizeCheck(unsigned int size_x, unsigned int size_y, double o_x, double o_y);
-
- /**
- * @brief Utility to share initialization code across constructors
- */
- void commonInit();
-
- /**
- * @brief Returns a 1D index into the MapCell array for a 2D index
- * @param x The desired x coordinate
- * @param y The desired y coordinate
- * @return The associated 1D index
- */
- size_t getIndex(int x, int y);
-
- /**
- * @brief Used to update the distance of a cell in path distance computation
- * @param current_cell The cell we're currently in
- * @param check_cell The cell to be updated
- */
- inline void updatePathCell(MapCell* current_cell, MapCell* check_cell,
- std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
- //mark the cell as visisted
- check_cell->path_mark = true;
-
- //if the cell is an obstacle set the max path distance
- unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
- if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
- check_cell->path_dist = map_.size();
- return;
- }
-
- double new_path_dist = current_cell->path_dist + 1;
- if(new_path_dist < check_cell->path_dist)
- check_cell->path_dist = new_path_dist;
-
- dist_queue.push(check_cell);
- }
-
- /**
- * @brief Used to update the distance of a cell in goal distance computation
- * @param current_cell The cell we're currently in
- * @param check_cell The cell to be updated
- */
- inline void updateGoalCell(MapCell* current_cell, MapCell* check_cell,
- std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap ){
- ///mark the cell as visited
- check_cell->goal_mark = true;
-
- //if the cell is an obstacle set the max goal distance
- unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
- if(!getCell(check_cell->cx, check_cell->cy).within_robot && (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || cost == costmap_2d::NO_INFORMATION)){
- check_cell->goal_dist = map_.size();
- return;
- }
-
- double new_goal_dist = current_cell->goal_dist + 1;
- if(new_goal_dist < check_cell->goal_dist)
- check_cell->goal_dist = new_goal_dist;
-
- dist_queue.push(check_cell);
- }
-
- /**
- * @brief Compute the distance from each cell in the local map grid to the planned path
- * @param dist_queue A queue of the initial cells on the path
- */
- void computePathDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
-
- /**
- * @brief Compute the distance from each cell in the local map grid to the local goal point
- * @param goal_queue A queue containing the local goal cell
- */
- void computeGoalDistance(std::queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap);
-
- /**
- * @brief Update what cells are considered path based on the global plan
- */
- void setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan);
-
- unsigned int size_x_, size_y_; ///< @brief The dimensions of the grid
- std::vector<MapCell> map_; ///< @brief Storage for the MapCells
-
- double scale; ///< @brief grid scale in meters/cell
-
- double goal_x_, goal_y_; /**< @brief The goal distance was last computed from */
-
- double origin_x, origin_y; ///< @brief lower left corner of grid in world space
- };
-};
-
-#endif
diff --git a/include/map_grid_cost_point.h b/include/map_grid_cost_point.h
deleted file mode 100755
index f03e74ced0f9bb1527f4c75acaded850ae5c6927..0000000000000000000000000000000000000000
--- a/include/map_grid_cost_point.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2010, Eric Perko
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Eric Perko nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef MAP_GRID_COST_POINT_H_
-#define MAP_GRID_COST_POINT_H_
-
-#include <pcl/ros/register_point_struct.h>
-
-namespace iri_ackermann_local_planner {
- struct MapGridCostPoint {
- float x;
- float y;
- float z;
- float path_cost;
- float goal_cost;
- float occ_cost;
- float total_cost;
- };
-}
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
- iri_ackermann_local_planner::MapGridCostPoint,
- (float, x, x)
- (float, y, y)
- (float, z, z)
- (float, path_cost, path_cost)
- (float, goal_cost, goal_cost)
- (float, occ_cost, occ_cost)
- (float, total_cost, total_cost));
-#endif
diff --git a/include/map_grid_visualizer.h b/include/map_grid_visualizer.h
deleted file mode 100755
index baaa74cb3243584d496b74bde52877b6b79ebd40..0000000000000000000000000000000000000000
--- a/include/map_grid_visualizer.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2010, Eric Perko
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Eric Perko nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef MAP_GRID_VISUALIZER_H_
-#define MAP_GRID_VISUALIZER_H_
-
-#include <ros/ros.h>
-#include <map_grid.h>
-#include <costmap_2d/costmap_2d.h>
-#include <map_grid_cost_point.h>
-#include <pcl_ros/publisher.h>
-
-namespace iri_ackermann_local_planner {
- class MapGridVisualizer {
- public:
- /**
- * @brief Default constructor
- */
- MapGridVisualizer();
-
- /**
- * @brief Initializes the MapGridVisualizer
- * @param name The name to be appended to ~/ in order to get the proper namespace for parameters
- * @param costmap The costmap instance to use to get the size of the map to generate a point cloud for
- * @param cost_function The function to use to compute the cost values to be inserted into each point in the output PointCloud as well as whether to include a given point or not
- */
- void initialize(const std::string& name,const costmap_2d::Costmap2D * costmap, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function);
-
- /**
- * Destructor for the visualizer
- */
- ~MapGridVisualizer() {}
-
- /**
- * @brief Build and publish a PointCloud if the publish_cost_grid_pc parameter was true. Only include points for which the cost_function at (cx,cy) returns true.
- */
- void publishCostCloud();
-
- private:
- std::string name_; ///< @brief The name to get parameters relative to.
- std::string frame_id_; ///< @brief The frame to assign to the output PointCloud
- boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function_; ///< @brief The function to be used to generate the cost components for the output PointCloud
- bool publish_cost_grid_pc_; ///< @brief Whether or not to build and publsih a PointCloud
- const costmap_2d::Costmap2D *costmap_p_; ///< @brief The costmap to use to infer the size of the underlying map grid
- ros::NodeHandle ns_nh_;
- pcl::PointCloud<MapGridCostPoint> cost_cloud_;
- pcl_ros::Publisher<MapGridCostPoint> pub_;
- };
-};
-
-#endif
diff --git a/include/planar_laser_scan.h b/include/planar_laser_scan.h
deleted file mode 100755
index 41d1c2b04f0205def9dc296c9a0c1ebe52a8d91a..0000000000000000000000000000000000000000
--- a/include/planar_laser_scan.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
-#define TRAJECTORY_ROLLOUT_PLANAR_LASER_SCAN_H_
-
-#include <geometry_msgs/Point32.h>
-#include <sensor_msgs/PointCloud.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class PlanarLaserScan
- * @brief Stores a scan from a planar laser that can be used to clear freespace
- */
- class PlanarLaserScan {
- public:
- PlanarLaserScan() {}
- geometry_msgs::Point32 origin;
- sensor_msgs::PointCloud cloud;
- double angle_min, angle_max, angle_increment;
- };
-};
-
-#endif
diff --git a/include/point_grid.h b/include/point_grid.h
deleted file mode 100755
index f460eae006fa6db855b7b99b70c871ab5c6eb9c3..0000000000000000000000000000000000000000
--- a/include/point_grid.h
+++ /dev/null
@@ -1,356 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef POINT_GRID_H_
-#define POINT_GRID_H_
-#include <vector>
-#include <list>
-#include <cfloat>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Point32.h>
-#include <costmap_2d/observation.h>
-#include <world_model.h>
-
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class PointGrid
- * @brief A class that implements the WorldModel interface to provide
- * free-space collision checks for the trajectory controller. This class
- * stores points binned into a grid and performs point-in-polygon checks when
- * necessary to determine the legality of a footprint at a given
- * position/orientation.
- */
- class PointGrid : public WorldModel {
- public:
- /**
- * @brief Constuctor for a grid that stores points in the plane
- * @param width The width in meters of the grid
- * @param height The height in meters of the gird
- * @param resolution The resolution of the grid in meters/cell
- * @param origin The origin of the bottom left corner of the grid
- * @param max_z The maximum height for an obstacle to be added to the grid
- * @param obstacle_range The maximum distance for obstacles to be added to the grid
- * @param min_separation The minimum distance between points in the grid
- */
- PointGrid(double width, double height, double resolution, geometry_msgs::Point origin,
- double max_z, double obstacle_range, double min_separation);
-
- /**
- * @brief Destructor for a point grid
- */
- virtual ~PointGrid(){}
-
- /**
- * @brief Returns the points that lie within the cells contained in the specified range. Some of these points may be outside the range itself.
- * @param lower_left The lower left corner of the range search
- * @param upper_right The upper right corner of the range search
- * @param points A vector of pointers to lists of the relevant points
- */
- void getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, std::vector< std::list<pcl::PointXYZ>* >& points);
-
- /**
- * @brief Checks if any points in the grid lie inside a convex footprint
- * @param position The position of the robot in world coordinates
- * @param footprint The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @return Positive if all the points lie outside the footprint, negative otherwise
- */
- virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius);
-
- /**
- * @brief Inserts observations from sensors into the point grid
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scans The laser scans used to clear freespace (the point grid only uses the first scan which is assumed to be the base laser)
- */
- void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
-
- /**
- * @brief Convert from world coordinates to grid coordinates
- * @param pt A point in world space
- * @param gx The x coordinate of the corresponding grid cell to be set by the function
- * @param gy The y coordinate of the corresponding grid cell to be set by the function
- * @return True if the conversion was successful, false otherwise
- */
- inline bool gridCoords(geometry_msgs::Point pt, unsigned int& gx, unsigned int& gy) const {
- if(pt.x < origin_.x || pt.y < origin_.y){
- gx = 0;
- gy = 0;
- return false;
- }
- gx = (int) ((pt.x - origin_.x)/resolution_);
- gy = (int) ((pt.y - origin_.y)/resolution_);
-
- if(gx >= width_ || gy >= height_){
- gx = 0;
- gy = 0;
- return false;
- }
-
- return true;
- }
-
- /**
- * @brief Get the bounds in world coordinates of a cell in the point grid, assumes a legal cell when called
- * @param gx The x coordinate of the grid cell
- * @param gy The y coordinate of the grid cell
- * @param lower_left The lower left bounds of the cell in world coordinates to be filled in
- * @param upper_right The upper right bounds of the cell in world coordinates to be filled in
- */
- inline void getCellBounds(unsigned int gx, unsigned int gy, geometry_msgs::Point& lower_left, geometry_msgs::Point& upper_right) const {
- lower_left.x = gx * resolution_ + origin_.x;
- lower_left.y = gy * resolution_ + origin_.y;
-
- upper_right.x = lower_left.x + resolution_;
- upper_right.y = lower_left.y + resolution_;
- }
-
-
- /**
- * @brief Compute the squared distance between two points
- * @param pt1 The first point
- * @param pt2 The second point
- * @return The squared distance between the two points
- */
- inline double sq_distance(pcl::PointXYZ& pt1, pcl::PointXYZ& pt2){
- return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y);
- }
-
- /**
- * @brief Convert from world coordinates to grid coordinates
- * @param pt A point in world space
- * @param gx The x coordinate of the corresponding grid cell to be set by the function
- * @param gy The y coordinate of the corresponding grid cell to be set by the function
- * @return True if the conversion was successful, false otherwise
- */
- inline bool gridCoords(pcl::PointXYZ pt, unsigned int& gx, unsigned int& gy) const {
- if(pt.x < origin_.x || pt.y < origin_.y){
- gx = 0;
- gy = 0;
- return false;
- }
- gx = (int) ((pt.x - origin_.x)/resolution_);
- gy = (int) ((pt.y - origin_.y)/resolution_);
-
- if(gx >= width_ || gy >= height_){
- gx = 0;
- gy = 0;
- return false;
- }
-
- return true;
- }
-
- /**
- * @brief Converts cell coordinates to an index value that can be used to look up the correct grid cell
- * @param gx The x coordinate of the cell
- * @param gy The y coordinate of the cell
- * @return The index of the cell in the stored cell list
- */
- inline unsigned int gridIndex(unsigned int gx, unsigned int gy) const {
- /*
- * (0, 0) ---------------------- (width, 0)
- * | |
- * | |
- * | |
- * | |
- * | |
- * (0, height) ----------------- (width, height)
- */
- return(gx + gy * width_);
- }
-
- /**
- * @brief Check the orientation of a pt c with respect to the vector a->b
- * @param a The start point of the vector
- * @param b The end point of the vector
- * @param c The point to compute orientation for
- * @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
- */
- inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b, const pcl::PointXYZ& c){
- double acx = a.x - c.x;
- double bcx = b.x - c.x;
- double acy = a.y - c.y;
- double bcy = b.y - c.y;
- return acx * bcy - acy * bcx;
- }
-
- /**
- * @brief Check the orientation of a pt c with respect to the vector a->b
- * @param a The start point of the vector
- * @param b The end point of the vector
- * @param c The point to compute orientation for
- * @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
- */
- inline double orient(const geometry_msgs::Point32& a, const geometry_msgs::Point32& b, const pcl::PointXYZ& c){
- double acx = a.x - c.x;
- double bcx = b.x - c.x;
- double acy = a.y - c.y;
- double bcy = b.y - c.y;
- return acx * bcy - acy * bcx;
- }
-
- /**
- * @brief Check the orientation of a pt c with respect to the vector a->b
- * @param a The start point of the vector
- * @param b The end point of the vector
- * @param c The point to compute orientation for
- * @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
- */
- inline double orient(const geometry_msgs::Point& a, const geometry_msgs::Point& b,
- const geometry_msgs::Point& c){
- double acx = a.x - c.x;
- double bcx = b.x - c.x;
- double acy = a.y - c.y;
- double bcy = b.y - c.y;
- return acx * bcy - acy * bcx;
- }
-
- /**
- * @brief Check the orientation of a pt c with respect to the vector a->b
- * @param a The start point of the vector
- * @param b The end point of the vector
- * @param c The point to compute orientation for
- * @return orient(a, b, c) < 0 ----> Right, orient(a, b, c) > 0 ----> Left
- */
- inline double orient(const pcl::PointXYZ& a, const pcl::PointXYZ& b, const pcl::PointXYZ& c){
- double acx = a.x - c.x;
- double bcx = b.x - c.x;
- double acy = a.y - c.y;
- double bcy = b.y - c.y;
- return acx * bcy - acy * bcx;
- }
-
- /**
- * @brief Check if two line segmenst intersect
- * @param v1 The first point of the first segment
- * @param v2 The second point of the first segment
- * @param u1 The first point of the second segment
- * @param u2 The second point of the second segment
- * @return True if the segments intersect, false otherwise
- */
- inline bool segIntersect(const pcl::PointXYZ& v1, const pcl::PointXYZ& v2,
- const pcl::PointXYZ& u1, const pcl::PointXYZ& u2){
- return (orient(v1, v2, u1) * orient(v1, v2, u2) < 0) && (orient(u1, u2, v1) * orient(u1, u2, v2) < 0);
- }
-
- /**
- * @brief Find the intersection point of two lines
- * @param v1 The first point of the first segment
- * @param v2 The second point of the first segment
- * @param u1 The first point of the second segment
- * @param u2 The second point of the second segment
- * @param result The point to be filled in
- */
- void intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
- const geometry_msgs::Point& u1, const geometry_msgs::Point& u2,
- geometry_msgs::Point& result);
-
- /**
- * @brief Check if a point is in a polygon
- * @param pt The point to be checked
- * @param poly The polygon to check against
- * @return True if the point is in the polygon, false otherwise
- */
- bool ptInPolygon(const pcl::PointXYZ& pt, const std::vector<geometry_msgs::Point>& poly);
-
- /**
- * @brief Insert a point into the point grid
- * @param pt The point to be inserted
- */
- void insert(pcl::PointXYZ pt);
-
- /**
- * @brief Find the distance between a point and its nearest neighbor in the grid
- * @param pt The point used for comparison
- * @return The distance between the point passed in and its nearest neighbor in the point grid
- */
- double nearestNeighborDistance(pcl::PointXYZ& pt);
-
- /**
- * @brief Find the distance between a point and its nearest neighbor in a cell
- * @param pt The point used for comparison
- * @param gx The x coordinate of the cell
- * @param gy The y coordinate of the cell
- * @return The distance between the point passed in and its nearest neighbor in the cell
- */
- double getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy);
-
- /**
- * @brief Removes points from the grid that lie within the polygon
- * @param poly A specification of the polygon to clear from the grid
- */
- void removePointsInPolygon(const std::vector<geometry_msgs::Point> poly);
-
- /**
- * @brief Removes points from the grid that lie within a laser scan
- * @param laser_scan A specification of the laser scan to use for clearing
- */
- void removePointsInScanBoundry(const PlanarLaserScan& laser_scan);
-
- /**
- * @brief Checks to see if a point is within a laser scan specification
- * @param pt The point to check
- * @param laser_scan The specification of the scan to check against
- * @return True if the point is contained within the scan, false otherwise
- */
- bool ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan);
-
- /**
- * @brief Get the points in the point grid
- * @param cloud The point cloud to insert the points into
- */
- void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
-
- private:
- double resolution_; ///< @brief The resolution of the grid in meters/cell
- geometry_msgs::Point origin_; ///< @brief The origin point of the grid
- unsigned int width_; ///< @brief The width of the grid in cells
- unsigned int height_; ///< @brief The height of the grid in cells
- std::vector< std::list<pcl::PointXYZ> > cells_; ///< @brief Storage for the cells in the grid
- double max_z_; ///< @brief The height cutoff for adding points as obstacles
- double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
- double sq_min_separation_; ///< @brief The minimum square distance required between points in the grid
- std::vector< std::list<pcl::PointXYZ>* > points_; ///< @brief The lists of points returned by a range search, made a member to save on memory allocation
- };
-};
-#endif
diff --git a/include/trajectory.h b/include/trajectory.h
deleted file mode 100755
index 1ac580788747862f62ef664a3206f4087f8e8561..0000000000000000000000000000000000000000
--- a/include/trajectory.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_H_
-#define TRAJECTORY_ROLLOUT_TRAJECTORY_H_
-
-#include <vector>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class Trajectory
- * @brief Holds a trajectory generated by an x, y, and theta velocity
- */
- class Trajectory {
- public:
- /**
- * @brief Default constructor
- */
- Trajectory();
-
- /**
- * @brief Constructs a trajectory
- * @param xv The x velocity used to seed the trajectory
- * @param yv The y velocity used to seed the trajectory
- * @param thetav The theta velocity used to seed the trajectory
- * @param num_pts The expected number of points for a trajectory
- */
- Trajectory(double xv, double yv, double thetav, unsigned int num_pts);
-
- double xv_, yv_, thetav_; ///< @brief The x, y, and theta velocities of the trajectory
-
- double cost_; ///< @brief The cost/score of the trajectory
-
- /**
- * @brief Get a point within the trajectory
- * @param index The index of the point to get
- * @param x Will be set to the x position of the point
- * @param y Will be set to the y position of the point
- * @param th Will be set to the theta position of the point
- */
- void getPoint(unsigned int index, double& x, double& y, double& th);
-
- /**
- * @brief Set a point within the trajectory
- * @param index The index of the point to set
- * @param x The x position
- * @param y The y position
- * @param th The theta position
- */
- void setPoint(unsigned int index, double x, double y, double th);
-
- /**
- * @brief Add a point to the end of a trajectory
- * @param x The x position
- * @param y The y position
- * @param th The theta position
- */
- void addPoint(double x, double y, double th);
-
- /**
- * @brief Get the last point of the trajectory
- * @param x Will be set to the x position of the point
- * @param y Will be set to the y position of the point
- * @param th Will be set to the theta position of the point
- */
- void getEndpoint(double& x, double& y, double& th);
-
- /**
- * @brief Clear the trajectory's points
- */
- void resetPoints();
-
- /**
- * @brief Return the number of points in the trajectory
- * @return The number of points in the trajectory
- */
- unsigned int getPointsSize();
-
- private:
- std::vector<double> x_pts_; ///< @brief The x points in the trajectory
- std::vector<double> y_pts_; ///< @brief The y points in the trajectory
- std::vector<double> th_pts_; ///< @brief The theta points in the trajectory
-
- };
-};
-#endif
diff --git a/include/trajectory_inc.h b/include/trajectory_inc.h
deleted file mode 100755
index 4d121b5b48aa5e8e54f1816a381d2973d047271a..0000000000000000000000000000000000000000
--- a/include/trajectory_inc.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#ifndef TRAJECTORY_INC_H_
-#define TRAJECTORY_INC_H_
-
-#include <limits>
-
-#ifndef DBL_MAX /* Max decimal value of a double */
-#define DBL_MAX std::numeric_limits<double>::max() // 1.7976931348623157e+308
-#endif
-
-#ifndef DBL_MIN //Min decimal value of a double
-#define DBL_MIN std::numeric_limits<double>::min() // 2.2250738585072014e-308
-#endif
-
-#endif
diff --git a/include/trajectory_planner.h b/include/trajectory_planner.h
deleted file mode 100755
index e4c9c46275ea44ec04d395849c70da89109a0526..0000000000000000000000000000000000000000
--- a/include/trajectory_planner.h
+++ /dev/null
@@ -1,322 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
-#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_H_
-
-#include <vector>
-#include <string>
-#include <sstream>
-#include <math.h>
-#include <ros/console.h>
-#include <angles/angles.h>
-
-//for creating a local cost grid
-#include <map_cell.h>
-#include <map_grid.h>
-
-//for obstacle data access
-#include <costmap_2d/costmap_2d.h>
-#include <world_model.h>
-
-#include <trajectory.h>
-
-//we'll take in a path as a vector of poses
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Twist.h>
-#include <iri_ackermann_local_planner/Position2DInt.h>
-
-//for computing path distance
-#include <queue>
-
-//for some datatypes
-#include <tf/transform_datatypes.h>
-
-#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
-#include <boost/algorithm/string.hpp>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class TrajectoryPlanner
- * @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world.
- */
- class TrajectoryPlanner{
- friend class TrajectoryPlannerTest; //Need this for gtest to work
- public:
- /**
- * @brief Constructs a trajectory controller
- * @param world_model The WorldModel the trajectory controller uses to check for collisions
- * @param costmap A reference to the Costmap the controller should use
- * @param footprint_spec A polygon representing the footprint of the robot. (Must be convex)
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @param acc_lim_x The acceleration limit of the robot in the x direction
- * @param acc_lim_y The acceleration limit of the robot in the y direction
- * @param acc_lim_theta The acceleration limit of the robot in the theta direction
- * @param sim_time The number of seconds to "roll-out" each trajectory
- * @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things
- * @param vx_samples The number of trajectories to sample in the x dimension
- * @param vtheta_samples The number of trajectories to sample in the theta dimension
- * @param pdist_scale A scaling factor for how close the robot should stay to the path
- * @param gdist_scale A scaling factor for how aggresively the robot should pursue a local goal
- * @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles
- * @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities
- * @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past
- * @param escape_reset_dist The distance the robot must travel before it can exit escape mode
- * @param escape_reset_theta The distance the robot must rotate before it can exit escape mode
- * @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise
- * @param max_vel_x The maximum x velocity the controller will explore
- * @param min_vel_x The minimum x velocity the controller will explore
- * @param max_vel_th The maximum rotational velocity the controller will explore
- * @param min_vel_th The minimum rotational velocity the controller will explore
- * @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore
- * @param backup_vel The velocity to use while backing up
- * @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits
- * @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep
- * @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories
- * @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation
- * @param y_vels A vector of the y velocities the controller will explore
- * @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things
- */
- TrajectoryPlanner(WorldModel& world_model,
- const costmap_2d::Costmap2D& costmap,
- std::vector<geometry_msgs::Point> footprint_spec,
- double max_acc = 1.0, double max_vel = 0.3, double min_vel = -0.3,
- double max_steer_acc = 1.0, double max_steer_vel = 0.5, double min_steer_vel = -0.5,
- double max_steer_angle = 0.35, double min_steer_angle = -0.35, double axis_distance = 1.65,
- double sim_time = 10.0, double sim_granularity = 0.025,
- int vx_samples = 20, int vtheta_samples = 20,
- double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.01, double hdiff_scale = 1.0,
- bool simple_attractor = false, double angular_sim_granularity = 0.025,int heading_points = 8, double xy_goal_tol = 0.5);
-
- /**
- * @brief Destructs a trajectory controller
- */
- ~TrajectoryPlanner();
-
- /**
- * @brief Reconfigures the trajectory planner
- */
- void reconfigure(AckermannLocalPlannerConfig &cfg);
-
- /**
- * @brief Given the current position, orientation, and velocity of the robot, return a trajectory to follow
- * @param global_pose The current pose of the robot in world space
- * @param global_vel The current velocity of the robot in world space
- * @param drive_velocities Will be set to velocities to send to the robot base
- * @return The selected path or trajectory
- */
- Trajectory findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities,geometry_msgs::Twist &ackermann_state);
-
- /**
- * @brief Update the plan that the controller is following
- * @param new_plan A new plan for the controller to follow
- * @param compute_dists Wheter or not to compute path/goal distances when a plan is updated
- */
- void updatePlan(const std::vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists = false);
-
- /**
- * @brief Accessor for the goal the robot is currently pursuing in world corrdinates
- * @param x Will be set to the x position of the local goal
- * @param y Will be set to the y position of the local goal
- */
- void getLocalGoal(double& x, double& y);
-
- /**
- * @brief Generate and score a single trajectory
- * @param x The x position of the robot
- * @param y The y position of the robot
- * @param theta The orientation of the robot
- * @param vx The x velocity of the robot
- * @param vy The y velocity of the robot
- * @param vtheta The theta velocity of the robot
- * @param vx_samp The x velocity used to seed the trajectory
- * @param vy_samp The y velocity used to seed the trajectory
- * @param vtheta_samp The theta velocity used to seed the trajectory
- * @return True if the trajectory is legal, false otherwise
- */
- bool checkTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
-
- /**
- * @brief Generate and score a single trajectory
- * @param x The x position of the robot
- * @param y The y position of the robot
- * @param theta The orientation of the robot
- * @param vx The x velocity of the robot
- * @param vy The y velocity of the robot
- * @param vtheta The theta velocity of the robot
- * @param vx_samp The x velocity used to seed the trajectory
- * @param vy_samp The y velocity used to seed the trajectory
- * @param vtheta_samp The theta velocity used to seed the trajectory
- * @return The score (as double)
- */
- double scoreTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp);
-
- /**
- * @brief Compute the components and total cost for a map grid cell
- * @param cx The x coordinate of the cell in the map grid
- * @param cy The y coordinate of the cell in the map grid
- * @param path_cost Will be set to the path distance component of the cost function
- * @param goal_cost Will be set to the goal distance component of the cost function
- * @param occ_cost Will be set to the costmap value of the cell
- * @param total_cost Will be set to the value of the overall cost function, taking into account the scaling parameters
- * @return True if the cell is traversible and therefore a legal location for the robot to move to
- */
- bool getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost);
- private:
- /**
- * @brief Create the trajectories we wish to explore, score them, and return the best option
- * @param x The x position of the robot
- * @param y The y position of the robot
- * @param theta The orientation of the robot
- * @param vx The x velocity of the robot
- * @param vy The y velocity of the robot
- * @param vtheta The theta velocity of the robot
- * @param acc_x The x acceleration limit of the robot
- * @param acc_y The y acceleration limit of the robot
- * @param acc_theta The theta acceleration limit of the robot
- * @return
- */
- Trajectory createTrajectories(double x, double y, double theta, double vx, double vy, double vtheta,
- double acc_x, double acc_y, double acc_theta);
-
- /**
- * @brief Generate and score a single trajectory
- * @param x The x position of the robot
- * @param y The y position of the robot
- * @param theta The orientation of the robot
- * @param vx The x velocity of the robot
- * @param vy The y velocity of the robot
- * @param vtheta The theta velocity of the robot
- * @param vx_samp The x velocity used to seed the trajectory
- * @param vy_samp The y velocity used to seed the trajectory
- * @param vtheta_samp The theta velocity used to seed the trajectory
- * @param acc_x The x acceleration limit of the robot
- * @param acc_y The y acceleration limit of the robot
- * @param acc_theta The theta acceleration limit of the robot
- * @param impossible_cost The cost value of a cell in the local map grid that is considered impassable
- * @param traj Will be set to the generated trajectory with its associated score
- */
- void generateTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y,
- double acc_theta, double impossible_cost, Trajectory& traj);
-
- /**
- * @brief Checks the legality of the robot footprint at a position and orientation using the world model
- * @param x_i The x position of the robot
- * @param y_i The y position of the robot
- * @param theta_i The orientation of the robot
- * @return
- */
- double footprintCost(double x_i, double y_i, double theta_i);
-
- /**
- * @brief Used to get the cells that make up the footprint of the robot
- * @param x_i The x position of the robot
- * @param y_i The y position of the robot
- * @param theta_i The orientation of the robot
- * @param fill If true: returns all cells in the footprint of the robot. If false: returns only the cells that make up the outline of the footprint.
- * @return The cells that make up either the outline or entire footprint of the robot depending on fill
- */
- std::vector<iri_ackermann_local_planner::Position2DInt> getFootprintCells(double x_i, double y_i, double theta_i, bool fill);
-
- /**
- * @brief Use Bresenham's algorithm to trace a line between two points in a grid
- * @param x0 The x coordinate of the first point
- * @param x1 The x coordinate of the second point
- * @param y0 The y coordinate of the first point
- * @param y1 The y coordinate of the second point
- * @param pts Will be filled with the cells that lie on the line in the grid
- */
- void getLineCells(int x0, int x1, int y0, int y1, std::vector<iri_ackermann_local_planner::Position2DInt>& pts);
-
- /**
- * @brief Fill the outline of a polygon, in this case the robot footprint, in a grid
- * @param footprint The list of cells making up the footprint in the grid, will be modified to include all cells inside the footprint
- */
- void getFillCells(std::vector<iri_ackermann_local_planner::Position2DInt>& footprint);
-
- MapGrid map_; ///< @brief The local map grid where we propagate goal and path distance
- const costmap_2d::Costmap2D& costmap_; ///< @brief Provides access to cost map information
- WorldModel& world_model_; ///< @brief The world model that the controller uses for collision detection
-
- std::vector<geometry_msgs::Point> footprint_spec_; ///< @brief The footprint specification of the robot
-
- std::vector<geometry_msgs::PoseStamped> global_plan_; ///< @brief The global path for the robot to follow
-
- double goal_x_,goal_y_; ///< @brief Storage for the local goal the robot is pursuing
-
- double sim_time_; ///< @brief The number of seconds each trajectory is "rolled-out"
- double sim_granularity_; ///< @brief The distance between simulation points
- double angular_sim_granularity_; ///< @brief The distance between angular simulation points
-
- int vx_samples_; ///< @brief The number of samples we'll take in the x dimenstion of the control space
- int vtheta_samples_; ///< @brief The number of samples we'll take in the theta dimension of the control space
-
- double pdist_scale_, gdist_scale_, occdist_scale_,hdiff_scale_; ///< @brief Scaling factors for the controller's cost function
-
- Trajectory traj_one, traj_two; ///< @brief Used for scoring trajectories
-
-
- /* ackerman parameters */
- double steering_speed_;
- /* ackerman reconfigure parameters */
- double ack_acc_max_;
- double ack_vel_min_;
- double ack_vel_max_;
- double ack_steer_acc_max_;
- double ack_steer_speed_max_;
- double ack_steer_speed_min_;
- double ack_steer_angle_max_;
- double ack_steer_angle_min_;
- double ack_axis_distance_;
-
- bool simple_attractor_; ///< @brief Enables simple attraction to a goal point
- int heading_points_;
- double xy_goal_tol_;
-
- boost::mutex configuration_mutex_;
-
- double lineCost(int x0, int x1, int y0, int y1);
- double pointCost(int x, int y);
- double headingDiff(int cell_x, int cell_y, double x, double y, double heading);
- };
-};
-
-#endif
diff --git a/include/trajectory_planner_ros.h b/include/trajectory_planner_ros.h
deleted file mode 100755
index a06e819d3adfb32f994efeb3554ed5f6bf011eba..0000000000000000000000000000000000000000
--- a/include/trajectory_planner_ros.h
+++ /dev/null
@@ -1,207 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
-#define TRAJECTORY_ROLLOUT_TRAJECTORY_PLANNER_ROS_H_
-
-#include <ros/ros.h>
-#include <costmap_2d/costmap_2d.h>
-#include <costmap_2d/costmap_2d_publisher.h>
-#include <costmap_2d/costmap_2d_ros.h>
-#include <world_model.h>
-#include <point_grid.h>
-#include <costmap_model.h>
-#include <voxel_grid_model.h>
-#include <trajectory_planner.h>
-#include <map_grid_visualizer.h>
-
-#include <planar_laser_scan.h>
-
-#include <tf/transform_datatypes.h>
-
-#include <nav_msgs/Odometry.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/Point.h>
-
-#include <tf/transform_listener.h>
-
-#include <boost/thread.hpp>
-
-#include <string>
-
-#include <angles/angles.h>
-
-#include <nav_core/base_local_planner.h>
-
-#include <dynamic_reconfigure/server.h>
-#include <iri_ackermann_local_planner/AckermannLocalPlannerConfig.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class TrajectoryPlannerROS
- * @brief A ROS wrapper for the trajectory controller that queries the param server to construct a controller
- */
- class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner {
- public:
- /**
- * @brief Default constructor for the ros wrapper
- */
- TrajectoryPlannerROS();
-
- /**
- * @brief Constructs the ros wrapper
- * @param name The name to give this instance of the trajectory planner
- * @param tf A pointer to a transform listener
- * @param costmap The cost map to use for assigning costs to trajectories
- */
- TrajectoryPlannerROS(std::string name, tf::TransformListener* tf,
- costmap_2d::Costmap2DROS* costmap_ros);
-
- /**
- * @brief Constructs the ros wrapper
- * @param name The name to give this instance of the trajectory planner
- * @param tf A pointer to a transform listener
- * @param costmap The cost map to use for assigning costs to trajectories
- */
- void initialize(std::string name, tf::TransformListener* tf,
- costmap_2d::Costmap2DROS* costmap_ros);
-
- /**
- * @brief Destructor for the wrapper
- */
- ~TrajectoryPlannerROS();
-
- /**
- * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
- * @param cmd_vel Will be filled with the velocity command to be passed to the robot base
- * @return True if a valid trajectory was found, false otherwise
- */
- bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
-
- /**
- * @brief Set the plan that the controller is following
- * @param orig_global_plan The plan to pass to the controller
- * @return True if the plan was updated successfully, false otherwise
- */
- bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
-
- /**
- * @brief Check if the goal pose has been achieved
- * @return True if achieved, false otherwise
- */
- bool isGoalReached();
-
- /**
- * @brief Generate and score a single trajectory
- * @param vx_samp The x velocity used to seed the trajectory
- * @param vy_samp The y velocity used to seed the trajectory
- * @param vtheta_samp The theta velocity used to seed the trajectory
- * @param update_map Whether or not to update the map for the planner
- * when computing the legality of the trajectory, this is useful to set
- * to false if you're going to be doing a lot of trajectory checking over
- * a short period of time
- * @return True if the trajectory is legal, false otherwise
- */
- bool checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
-
- /**
- * @brief Generate and score a single trajectory
- * @param vx_samp The x velocity used to seed the trajectory
- * @param vy_samp The y velocity used to seed the trajectory
- * @param vtheta_samp The theta velocity used to seed the trajectory
- * @param update_map Whether or not to update the map for the planner
- * when computing the legality of the trajectory, this is useful to set
- * to false if you're going to be doing a lot of trajectory checking over
- * a short period of time
- * @return score of trajectory (double)
- */
- double scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map = true);
-
- private:
- /**
- * @brief Callback for dynamic_reconfigure
- */
- void reconfigureCB(AckermannLocalPlannerConfig &config, uint32_t level);
-
- /**
- * @brief Stop the robot taking into account acceleration limits
- * @param global_pose The pose of the robot in the global frame
- * @param robot_vel The velocity of the robot
- * @param cmd_vel The velocity commands to be filled
- * @return True if a valid trajectory was found, false otherwise
- */
- void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
-
- double sign(double x){
- return x < 0.0 ? -1.0 : 1.0;
- }
-
- WorldModel* world_model_; ///< @brief The world model that the controller will use
- TrajectoryPlanner* tc_; ///< @brief The trajectory controller
- costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
- costmap_2d::Costmap2D costmap_; ///< @brief The costmap the controller will use
- MapGridVisualizer map_viz_; ///< @brief The map grid visualizer for outputting the potential field generated by the cost function
- tf::TransformListener* tf_; ///< @brief Used for transforming point clouds
- std::string global_frame_; ///< @brief The frame in which the controller will run
- double max_sensor_range_; ///< @brief Keep track of the effective maximum range of our sensors
- nav_msgs::Odometry base_odom_; ///< @brief Used to get the velocity of the robot
- std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
- double rot_stopped_velocity_, trans_stopped_velocity_;
- double xy_goal_tolerance_, yaw_goal_tolerance_;
- double inflation_radius_;
- std::vector<geometry_msgs::PoseStamped> global_plan_;
- bool prune_plan_;
- ros::Publisher g_plan_pub_, l_plan_pub_;
- ros::Subscriber odom_sub_;
- boost::recursive_mutex odom_lock_;
- bool initialized_;
- bool rotating_to_goal_;
- bool latch_xy_goal_tolerance_, xy_tolerance_latch_;
- dynamic_reconfigure::Server<AckermannLocalPlannerConfig> *dsrv_;
- iri_ackermann_local_planner::AckermannLocalPlannerConfig default_config_;
- bool setup_;
-
- std::vector < std::vector < geometry_msgs::PoseStamped > > subPathList;
- std::vector < geometry_msgs::PoseStamped > subPath;
- unsigned int subPathIndex;
-
- geometry_msgs::Twist ackermann_state_;
- };
-
-};
-
-#endif
diff --git a/include/voxel_grid_model.h b/include/voxel_grid_model.h
deleted file mode 100755
index 625dd99a91cab6a419db8e52e74c64635ea7bf1a..0000000000000000000000000000000000000000
--- a/include/voxel_grid_model.h
+++ /dev/null
@@ -1,176 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
-#define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
-#include <vector>
-#include <list>
-#include <cfloat>
-#include <geometry_msgs/Point32.h>
-#include <costmap_2d/observation.h>
-#include <world_model.h>
-
-#include <pcl/point_types.h>
-#include <pcl/point_cloud.h>
-
-//voxel grid stuff
-#include <voxel_grid/voxel_grid.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class VoxelGridModel
- * @brief A class that implements the WorldModel interface to provide grid
- * based collision checks for the trajectory controller using a 3D voxel grid.
- */
- class VoxelGridModel : public WorldModel {
- public:
- /**
- * @brief Constructor for the VoxelGridModel
- * @param size_x The x size of the map
- * @param size_y The y size of the map
- * @param size_z The z size of the map... up to 32 cells
- * @param xy_resolution The horizontal resolution of the map in meters/cell
- * @param z_resolution The vertical resolution of the map in meters/cell
- * @param origin_x The x value of the origin of the map
- * @param origin_y The y value of the origin of the map
- * @param origin_z The z value of the origin of the map
- * @param max_z The maximum height for an obstacle to be added to the grid
- * @param obstacle_range The maximum distance for obstacles to be added to the grid
- */
- VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
- double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
-
- /**
- * @brief Destructor for the world model
- */
- virtual ~VoxelGridModel(){}
-
- /**
- * @brief Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the grid
- * @param position The position of the robot in world coordinates
- * @param footprint The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @return Positive if all the points lie outside the footprint, negative otherwise
- */
- virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius);
-
- /**
- * @brief The costmap already keeps track of world observations, so for this world model this method does nothing
- * @param footprint The footprint of the robot in its current location
- * @param observations The observations from various sensors
- * @param laser_scan The scans used to clear freespace
- */
- void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
- const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
-
- void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
-
- private:
- /**
- * @brief Rasterizes a line in the costmap grid and checks for collisions
- * @param x0 The x position of the first cell in grid coordinates
- * @param y0 The y position of the first cell in grid coordinates
- * @param x1 The x position of the second cell in grid coordinates
- * @param y1 The y position of the second cell in grid coordinates
- * @return A positive cost for a legal line... negative otherwise
- */
- double lineCost(int x0, int x1, int y0, int y1);
-
- /**
- * @brief Checks the cost of a point in the costmap
- * @param x The x position of the point in cell coordinates
- * @param y The y position of the point in cell coordinates
- * @return A positive cost for a legal point... negative otherwise
- */
- double pointCost(int x, int y);
-
- void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
-
- inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
- if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
- return false;
- mx = (int) ((wx - origin_x_) / xy_resolution_);
- my = (int) ((wy - origin_y_) / xy_resolution_);
- mz = (int) ((wz - origin_z_) / z_resolution_);
- return true;
- }
-
- inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
- if(wx < origin_x_ || wy < origin_y_)
- return false;
- mx = (int) ((wx - origin_x_) / xy_resolution_);
- my = (int) ((wy - origin_y_) / xy_resolution_);
- return true;
- }
-
- inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
- //returns the center point of the cell
- wx = origin_x_ + (mx + 0.5) * xy_resolution_;
- wy = origin_y_ + (my + 0.5) * xy_resolution_;
- wz = origin_z_ + (mz + 0.5) * z_resolution_;
- }
-
- inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
- //returns the center point of the cell
- wx = origin_x_ + (mx + 0.5) * xy_resolution_;
- wy = origin_y_ + (my + 0.5) * xy_resolution_;
- }
-
- inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
- return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
- }
-
- inline void insert(pcl::PointXYZ pt){
- unsigned int cell_x, cell_y, cell_z;
- if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
- return;
- obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
- }
-
- voxel_grid::VoxelGrid obstacle_grid_;
- double xy_resolution_;
- double z_resolution_;
- double origin_x_;
- double origin_y_;
- double origin_z_;
- double max_z_; ///< @brief The height cutoff for adding points as obstacles
- double sq_obstacle_range_; ///< @brief The square distance at which we no longer add obstacles to the grid
-
- };
-};
-#endif
diff --git a/include/world_model.h b/include/world_model.h
deleted file mode 100755
index dc5eab13a0e515669e9fe771fe4d2506847c5724..0000000000000000000000000000000000000000
--- a/include/world_model.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#ifndef TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
-#define TRAJECTORY_ROLLOUT_WORLD_MODEL_H_
-
-#include <vector>
-#include <costmap_2d/observation.h>
-#include <geometry_msgs/Point.h>
-#include <planar_laser_scan.h>
-
-namespace iri_ackermann_local_planner {
- /**
- * @class WorldModel
- * @brief An interface the trajectory controller uses to interact with the
- * world regardless of the underlying world model.
- */
- class WorldModel{
- public:
- /**
- * @brief Subclass will implement this method to check a footprint at a given position and orientation for legality in the world
- * @param position The position of the robot in world coordinates
- * @param footprint The specification of the footprint of the robot in world coordinates
- * @param inscribed_radius The radius of the inscribed circle of the robot
- * @param circumscribed_radius The radius of the circumscribed circle of the robot
- * @return Positive if all the points lie outside the footprint, negative otherwise
- */
- virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius) = 0;
-
- /**
- * @brief Subclass will implement a destructor
- */
- virtual ~WorldModel(){}
-
- protected:
- WorldModel(){}
- };
-
-};
-#endif
diff --git a/manifest.xml b/manifest.xml
deleted file mode 100755
index 0a869e1809534a3060ee86737a6882ad6accb3bc..0000000000000000000000000000000000000000
--- a/manifest.xml
+++ /dev/null
@@ -1,35 +0,0 @@
-<package>
- <description brief="iri_ackermann_local_planner">
-
- iri_ackermann_local_planner
-
- </description>
- <author>Sergi Hernandez Juan</author>
- <license>LGPL</license>
- <review status="unreviewed" notes=""/>
- <url>http://ros.org/wiki/iri_ackermann_local_planner</url>
- <depend package="std_msgs" />
- <depend package="nav_msgs" />
- <depend package="rosconsole" />
- <depend package="roscpp" />
- <depend package="tf" />
- <depend package="roslib" />
- <depend package="rospy" />
- <depend package="pluginlib" />
- <depend package="costmap_2d" />
- <depend package="voxel_grid" />
- <depend package="angles" />
- <depend package="visualization_msgs"/>
- <depend package="geometry_msgs"/>
- <depend package="nav_core"/>
- <depend package="pcl"/>
- <depend package="pcl_ros"/>
- <depend package="common_rosdeps" />
- <rosdep name="eigen" />
- <export>
- <cpp cflags="-I${prefix}/include -I${prefix}/cfg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -liri_ackermann_local_planner" />
- <nav_core plugin="${prefix}/ackermannlp_plugin.xml" />
- </export>
-</package>
-
-
diff --git a/msg/Position2DInt.msg b/msg/Position2DInt.msg
deleted file mode 100755
index 7d5c79921769e015fcf28488a25a217674e6353d..0000000000000000000000000000000000000000
--- a/msg/Position2DInt.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-int64 x
-int64 y
\ No newline at end of file
diff --git a/src/costmap_model.cpp b/src/costmap_model.cpp
deleted file mode 100755
index 91fb92aa6594160c109f4b30160753c36dc8ec80..0000000000000000000000000000000000000000
--- a/src/costmap_model.cpp
+++ /dev/null
@@ -1,197 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#include <costmap_model.h>
-
-using namespace std;
-using namespace costmap_2d;
-
-namespace iri_ackermann_local_planner {
- CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
-
- double CostmapModel::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius){
- //used to put things into grid coordinates
- unsigned int cell_x, cell_y;
-
- //get the cell coord of the center point of the robot
- if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
- return -1.0;
-
- //if number of points in the footprint is less than 3, we'll just assume a circular robot
- if(footprint.size() < 3){
- unsigned char cost = costmap_.getCost(cell_x, cell_y);
- //if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
- if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION)
- return -1.0;
- return cost;
- }
-
- //now we really have to lay down the footprint in the costmap grid
- unsigned int x0, x1, y0, y1;
- double line_cost = 0.0;
- double footprint_cost = 0.0;
-
- //we need to rasterize each line in the footprint
- for(unsigned int i = 0; i < footprint.size() - 1; ++i){
- //get the cell coord of the first point
- if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
- return -1.0;
-
- //get the cell coord of the second point
- if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
- return -1.0;
-
- line_cost = lineCost(x0, x1, y0, y1);
- footprint_cost = std::max(line_cost, footprint_cost);
-
- //if there is an obstacle that hits the line... we know that we can return false right away
- if(line_cost < 0)
- return -1.0;
- }
-
- //we also need to connect the first point in the footprint to the last point
- //get the cell coord of the last point
- if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
- return -1.0;
-
- //get the cell coord of the first point
- if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
- return -1.0;
-
- line_cost = lineCost(x0, x1, y0, y1);
- footprint_cost = std::max(line_cost, footprint_cost);
-
- if(line_cost < 0)
- return -1.0;
-
- //if all line costs are legal... then we can return that the footprint is legal
- return footprint_cost;
-
- }
-
- //calculate the cost of a ray-traced line
- double CostmapModel::lineCost(int x0, int x1,
- int y0, int y1){
- //Bresenham Ray-Tracing
- int deltax = abs(x1 - x0); // The difference between the x's
- int deltay = abs(y1 - y0); // The difference between the y's
- int x = x0; // Start x off at the first pixel
- int y = y0; // Start y off at the first pixel
-
- int xinc1, xinc2, yinc1, yinc2;
- int den, num, numadd, numpixels;
-
- double line_cost = 0.0;
- double point_cost = -1.0;
-
- if (x1 >= x0) // The x-values are increasing
- {
- xinc1 = 1;
- xinc2 = 1;
- }
- else // The x-values are decreasing
- {
- xinc1 = -1;
- xinc2 = -1;
- }
-
- if (y1 >= y0) // The y-values are increasing
- {
- yinc1 = 1;
- yinc2 = 1;
- }
- else // The y-values are decreasing
- {
- yinc1 = -1;
- yinc2 = -1;
- }
-
- if (deltax >= deltay) // There is at least one x-value for every y-value
- {
- xinc1 = 0; // Don't change the x when numerator >= denominator
- yinc2 = 0; // Don't change the y for every iteration
- den = deltax;
- num = deltax / 2;
- numadd = deltay;
- numpixels = deltax; // There are more x-values than y-values
- }
- else // There is at least one y-value for every x-value
- {
- xinc2 = 0; // Don't change the x for every iteration
- yinc1 = 0; // Don't change the y when numerator >= denominator
- den = deltay;
- num = deltay / 2;
- numadd = deltax;
- numpixels = deltay; // There are more y-values than x-values
- }
-
- for (int curpixel = 0; curpixel <= numpixels; curpixel++)
- {
- point_cost = pointCost(x, y); //Score the current point
-
- if(point_cost < 0)
- return -1;
-
- if(line_cost < point_cost)
- line_cost = point_cost;
-
- num += numadd; // Increase the numerator by the top of the fraction
- if (num >= den) // Check if numerator >= denominator
- {
- num -= den; // Calculate the new numerator value
- x += xinc1; // Change the x as appropriate
- y += yinc1; // Change the y as appropriate
- }
- x += xinc2; // Change the x as appropriate
- y += yinc2; // Change the y as appropriate
- }
-
- return line_cost;
- }
-
- double CostmapModel::pointCost(int x, int y){
- unsigned char cost = costmap_.getCost(x, y);
- //if the cell is in an obstacle the path is invalid
- //if(cost == LETHAL_OBSTACLE){
- if(cost == LETHAL_OBSTACLE || cost == NO_INFORMATION){
- return -1;
- }
-
- return cost;
- }
-
-};
diff --git a/src/goal_functions.cpp b/src/goal_functions.cpp
deleted file mode 100755
index 36562fa58d5be656d7b8d01a8e7a7825bd2cdadd..0000000000000000000000000000000000000000
--- a/src/goal_functions.cpp
+++ /dev/null
@@ -1,249 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2009, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#include <goal_functions.h>
-
-namespace iri_ackermann_local_planner {
- double distance(double x1, double y1, double x2, double y2){
- return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
- }
-
- bool goalPositionReached(const tf::Stamped<tf::Pose>& global_pose, double goal_x, double goal_y, double xy_goal_tolerance){
- double dist = distance(global_pose.getOrigin().x(), global_pose.getOrigin().y(), goal_x, goal_y);
- return fabs(dist) <= xy_goal_tolerance;
- }
-
- bool goalOrientationReached(const tf::Stamped<tf::Pose>& global_pose, double goal_th, double yaw_goal_tolerance){
- double yaw = tf::getYaw(global_pose.getRotation());
- return fabs(angles::shortest_angular_distance(yaw, goal_th)) <= yaw_goal_tolerance;
- }
-
- void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a){
- //given an empty path we won't do anything
- if(path.empty())
- return;
-
- //create a path message
- nav_msgs::Path gui_path;
- gui_path.poses.resize(path.size());
- gui_path.header.frame_id = path[0].header.frame_id;
- gui_path.header.stamp = path[0].header.stamp;
-
- // Extract the plan in world co-ordinates, we assume the path is all in the same frame
- for(unsigned int i=0; i < path.size(); i++){
- gui_path.poses[i] = path[i];
- }
-
- pub.publish(gui_path);
- }
-
- void prunePlan(const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
- ROS_ASSERT(global_plan.size() >= plan.size());
- std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
- std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
- while(it != plan.end()){
- const geometry_msgs::PoseStamped& w = *it;
- // Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
- double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
- double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
- double distance_sq = x_diff * x_diff + y_diff * y_diff;
- if(distance_sq < 1){
- ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
- break;
- }
- it = plan.erase(it);
- global_it = global_plan.erase(global_it);
- }
- }
-
- bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
- const costmap_2d::Costmap2DROS& costmap, const std::string& global_frame,
- std::vector<geometry_msgs::PoseStamped>& transformed_plan){
- const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
-
- transformed_plan.clear();
-
- try{
- if (!global_plan.size() > 0)
- {
- ROS_ERROR("Recieved plan with zero length");
- return false;
- }
-
- tf::StampedTransform transform;
- tf.lookupTransform(global_frame, ros::Time(),
- plan_pose.header.frame_id, plan_pose.header.stamp,
- plan_pose.header.frame_id, transform);
-
- //let's get the pose of the robot in the frame of the plan
- tf::Stamped<tf::Pose> robot_pose;
- robot_pose.setIdentity();
- robot_pose.frame_id_ = costmap.getBaseFrameID();
- robot_pose.stamp_ = ros::Time();
- tf.transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
-
- //we'll keep points on the plan that are within the window that we're looking at
- double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
-
- unsigned int i = 0;
- double sq_dist_threshold = dist_threshold * dist_threshold;
- double sq_dist = DBL_MAX;
-
- //we need to loop to a point on the plan that is within a certain distance of the robot
- while(i < (unsigned int)global_plan.size() && sq_dist > sq_dist_threshold){
- double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
- double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
- sq_dist = x_diff * x_diff + y_diff * y_diff;
- ++i;
- }
-
- //make sure not to count the first point that is too far away
- if(i > 0)
- --i;
-
- tf::Stamped<tf::Pose> tf_pose;
- geometry_msgs::PoseStamped newer_pose;
-
- //now we'll transform until points are outside of our distance threshold
- while(i < (unsigned int)global_plan.size() && sq_dist < sq_dist_threshold){
- double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
- double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
- sq_dist = x_diff * x_diff + y_diff * y_diff;
-
- const geometry_msgs::PoseStamped& pose = global_plan[i];
- poseStampedMsgToTF(pose, tf_pose);
- tf_pose.setData(transform * tf_pose);
- tf_pose.stamp_ = transform.stamp_;
- tf_pose.frame_id_ = global_frame;
- poseStampedTFToMsg(tf_pose, newer_pose);
-
- transformed_plan.push_back(newer_pose);
-
- ++i;
- }
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return false;
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return false;
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (global_plan.size() > 0)
- ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
-
- return false;
- }
-
- return true;
- }
-
- bool isGoalReached(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
- const costmap_2d::Costmap2DROS& costmap_ros, const std::string& global_frame,
- const nav_msgs::Odometry& base_odom, double rot_stopped_vel, double trans_stopped_vel,
- double xy_goal_tolerance, double yaw_goal_tolerance){
- const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
- tf::Stamped<tf::Pose> goal_pose;
-
- try{
- if (!global_plan.size() > 0)
- {
- ROS_ERROR("Recieved plan with zero length");
- return false;
- }
-
- tf::StampedTransform transform;
- tf.lookupTransform(global_frame, ros::Time(),
- plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
- plan_goal_pose.header.frame_id, transform);
-
- poseStampedMsgToTF(plan_goal_pose, goal_pose);
- goal_pose.setData(transform * goal_pose);
- goal_pose.stamp_ = transform.stamp_;
- goal_pose.frame_id_ = global_frame;
-
- }
- catch(tf::LookupException& ex) {
- ROS_ERROR("No Transform available Error: %s\n", ex.what());
- return false;
- }
- catch(tf::ConnectivityException& ex) {
- ROS_ERROR("Connectivity Error: %s\n", ex.what());
- return false;
- }
- catch(tf::ExtrapolationException& ex) {
- ROS_ERROR("Extrapolation Error: %s\n", ex.what());
- if (global_plan.size() > 0)
- ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
-
- return false;
- }
-
- //we assume the global goal is the last point in the global plan
- double goal_x = goal_pose.getOrigin().getX();
- double goal_y = goal_pose.getOrigin().getY();
-
- double yaw = tf::getYaw(goal_pose.getRotation());
-
- double goal_th = yaw;
-
- tf::Stamped<tf::Pose> global_pose;
- if(!costmap_ros.getRobotPose(global_pose))
- return false;
-
- //check to see if we've reached the goal position
- if(goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance)){
- //check to see if the goal orientation has been reached
- if(goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance)){
- //make sure that we're actually stopped before returning success
- if(stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
- return true;
- }
- }
-
- return false;
- }
-
- bool stopped(const nav_msgs::Odometry& base_odom,
- const double& rot_stopped_velocity, const double& trans_stopped_velocity){
- return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
- && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
- && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
- }
-};
diff --git a/src/map_cell.cpp b/src/map_cell.cpp
deleted file mode 100755
index beef76714e558cec2d80777ed19e715baeca4c26..0000000000000000000000000000000000000000
--- a/src/map_cell.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-#include <map_cell.h>
-
-namespace iri_ackermann_local_planner{
-
- MapCell::MapCell()
- : cx(0), cy(0), path_dist(DBL_MAX), goal_dist(DBL_MAX), occ_dist(0.0), occ_state(0), path_mark(false), goal_mark(false), within_robot(false)
- {}
-
- MapCell::MapCell(const MapCell& mc)
- : cx(mc.cx), cy(mc.cy), path_dist(mc.path_dist), goal_dist(mc.goal_dist),
- occ_dist(mc.occ_dist), occ_state(mc.occ_state), path_mark(mc.path_mark),
- goal_mark(mc.goal_mark), within_robot(mc.within_robot)
- {}
-};
diff --git a/src/map_grid.cpp b/src/map_grid.cpp
deleted file mode 100755
index d4557a2c3c65dd3761328093dd65e252b1d96b7d..0000000000000000000000000000000000000000
--- a/src/map_grid.cpp
+++ /dev/null
@@ -1,244 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#include <map_grid.h>
-
-using namespace std;
-
-namespace iri_ackermann_local_planner{
-
- MapGrid::MapGrid()
- : size_x_(0), size_y_(0)
- {
- }
-
- MapGrid::MapGrid(unsigned int size_x, unsigned int size_y)
- : size_x_(size_x), size_y_(size_y)
- {
- commonInit();
- }
-
- MapGrid::MapGrid(unsigned int size_x, unsigned int size_y, double s, double x, double y)
- : size_x_(size_x), size_y_(size_y), scale(s), origin_x(x), origin_y(y)
- {
- commonInit();
- }
-
- MapGrid::MapGrid(const MapGrid& mg){
- size_y_ = mg.size_y_;
- size_x_ = mg.size_x_;
- map_ = mg.map_;
- }
-
- void MapGrid::commonInit(){
- //don't allow construction of zero size grid
- ROS_ASSERT(size_y_ != 0 && size_x_ != 0);
-
- map_.resize(size_y_ * size_x_);
-
- //make each cell aware of its location in the grid
- for(unsigned int i = 0; i < size_y_; ++i){
- for(unsigned int j = 0; j < size_x_; ++j){
- unsigned int id = size_x_ * i + j;
- map_[id].cx = j;
- map_[id].cy = i;
- }
- }
- }
-
- size_t MapGrid::getIndex(int x, int y){
- return size_x_ * y + x;
- }
-
- MapGrid& MapGrid::operator= (const MapGrid& mg){
- size_y_ = mg.size_y_;
- size_x_ = mg.size_x_;
- map_ = mg.map_;
- return *this;
- }
-
- void MapGrid::sizeCheck(unsigned int size_x, unsigned int size_y, double o_x, double o_y){
- if(map_.size() != size_x * size_y)
- map_.resize(size_x * size_y);
-
- if(size_x_ != size_x || size_y_ != size_y){
- size_x_ = size_x;
- size_y_ = size_y;
-
- for(unsigned int i = 0; i < size_y_; ++i){
- for(unsigned int j = 0; j < size_x_; ++j){
- int index = size_x_ * i + j;
- map_[index].cx = j;
- map_[index].cy = i;
- }
- }
- }
- origin_x = o_x;
- origin_y = o_y;
- }
-
- //reset the path_dist and goal_dist fields for all cells
- void MapGrid::resetPathDist(){
- for(unsigned int i = 0; i < map_.size(); ++i){
- map_[i].path_dist = DBL_MAX;
- map_[i].goal_dist = DBL_MAX;
- map_[i].path_mark = false;
- map_[i].goal_mark = false;
- map_[i].within_robot = false;
- }
- }
-
- //update what map cells are considered path based on the global_plan
- void MapGrid::setPathCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan){
- sizeCheck(costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getOriginX(), costmap.getOriginY());
- int local_goal_x = -1;
- int local_goal_y = -1;
- bool started_path = false;
- queue<MapCell*> path_dist_queue;
- queue<MapCell*> goal_dist_queue;
- for(unsigned int i = 0; i < global_plan.size(); ++i){
- double g_x = global_plan[i].pose.position.x;
- double g_y = global_plan[i].pose.position.y;
- unsigned int map_x, map_y;
- if(costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION){
- MapCell& current = getCell(map_x, map_y);
- current.path_dist = 0.0;
- current.path_mark = true;
- path_dist_queue.push(¤t);
- local_goal_x = map_x;
- local_goal_y = map_y;
- started_path = true;
- }
- else{
- if(started_path)
- break;
- }
- }
-
- if(local_goal_x >= 0 && local_goal_y >= 0){
- MapCell& current = getCell(local_goal_x, local_goal_y);
- costmap.mapToWorld(local_goal_x, local_goal_y, goal_x_, goal_y_);
- current.goal_dist = 0.0;
- current.goal_mark = true;
- goal_dist_queue.push(¤t);
- }
- ROS_INFO("MapGrid::setPathCells: Local goal: %f, %f", goal_x_, goal_y_);
-
- //compute our distances
- computePathDistance(path_dist_queue, costmap);
- computeGoalDistance(goal_dist_queue, costmap);
- }
-
- void MapGrid::computePathDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
- MapCell* current_cell;
- MapCell* check_cell;
- unsigned int last_col = size_x_ - 1;
- unsigned int last_row = size_y_ - 1;
- while(!dist_queue.empty()){
- current_cell = dist_queue.front();
- check_cell = current_cell;
- dist_queue.pop();
-
- if(current_cell->cx > 0){
- check_cell = current_cell - 1;
- if(!check_cell->path_mark){
- updatePathCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cx < last_col){
- check_cell = current_cell + 1;
- if(!check_cell->path_mark){
- updatePathCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cy > 0){
- check_cell = current_cell - size_x_;
- if(!check_cell->path_mark){
- updatePathCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cy < last_row){
- check_cell = current_cell + size_x_;
- if(!check_cell->path_mark){
- updatePathCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
- }
- }
-
- void MapGrid::computeGoalDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
- MapCell* current_cell;
- MapCell* check_cell;
- unsigned int last_col = size_x_ - 1;
- unsigned int last_row = size_y_ - 1;
- while(!dist_queue.empty()){
- current_cell = dist_queue.front();
- current_cell->goal_mark = true;
- check_cell = current_cell;
- dist_queue.pop();
-
- if(current_cell->cx > 0){
- check_cell = current_cell - 1;
- if(!check_cell->goal_mark){
- updateGoalCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cx < last_col){
- check_cell = current_cell + 1;
- if(!check_cell->goal_mark){
- updateGoalCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cy > 0){
- check_cell = current_cell - size_x_;
- if(!check_cell->goal_mark){
- updateGoalCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
-
- if(current_cell->cy < last_row){
- check_cell = current_cell + size_x_;
- if(!check_cell->goal_mark){
- updateGoalCell(current_cell, check_cell, dist_queue, costmap);
- }
- }
- }
- }
-
-
-};
diff --git a/src/map_grid_visualizer.cpp b/src/map_grid_visualizer.cpp
deleted file mode 100755
index 786ecad16f1b614d577ffc3864b03c242e1dd06c..0000000000000000000000000000000000000000
--- a/src/map_grid_visualizer.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2010, Eric Perko
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of Eric Perko nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#include <map_grid_visualizer.h>
-#include <map_cell.h>
-#include <vector>
-
-namespace iri_ackermann_local_planner {
- MapGridVisualizer::MapGridVisualizer() {}
-
-
- void MapGridVisualizer::initialize(const std::string& name,const costmap_2d::Costmap2D * costmap, boost::function<bool (int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost)> cost_function) {
- name_ = name;
- costmap_p_ = costmap;
- cost_function_ = cost_function;
-
- ns_nh_ = ros::NodeHandle("~/" + name_);
- ns_nh_.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
- ns_nh_.param("global_frame_id", frame_id_, std::string("odom"));
-
- cost_cloud_.header.frame_id = frame_id_;
- pub_.advertise(ns_nh_, "cost_cloud", 1);
- }
-
- void MapGridVisualizer::publishCostCloud() {
- if(publish_cost_grid_pc_) {
- unsigned int x_size = costmap_p_->getSizeInCellsX();
- unsigned int y_size = costmap_p_->getSizeInCellsY();
- double z_coord = 0.0;
- double x_coord, y_coord;
- MapGridCostPoint pt;
- cost_cloud_.points.clear();
- cost_cloud_.header.stamp = ros::Time::now();
- float path_cost, goal_cost, occ_cost, total_cost;
- for (unsigned int cx = 0; cx < x_size; cx++) {
- for(unsigned int cy = 0; cy < y_size; cy++) {
- costmap_p_->mapToWorld(cx, cy, x_coord, y_coord);
- if(cost_function_(cx, cy, path_cost, goal_cost, occ_cost, total_cost)) {
- pt.x = x_coord;
- pt.y = y_coord;
- pt.z = z_coord;
- pt.path_cost = path_cost;
- pt.goal_cost = goal_cost;
- pt.occ_cost = occ_cost;
- pt.total_cost = total_cost;
- cost_cloud_.push_back(pt);
- }
- }
- }
- pub_.publish(cost_cloud_);
- ROS_DEBUG("Cost PointCloud published");
- }
- }
-};
diff --git a/src/point_grid.cpp b/src/point_grid.cpp
deleted file mode 100755
index b01b66d814c1181bdc0cd1946eb268936bd6e6ce..0000000000000000000000000000000000000000
--- a/src/point_grid.cpp
+++ /dev/null
@@ -1,688 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-
-#include <point_grid.h>
-#include <ros/console.h>
-#include <sys/time.h>
-#include <math.h>
-#include <cstdio>
-
-using namespace std;
-using namespace costmap_2d;
-
-void printPoint(pcl::PointXYZ pt){
- printf("(%.2f, %.2f, %.2f)", pt.x, pt.y, pt.z);
-}
-
-void printPSHeader(){
- printf("%%!PS\n");
- printf("%%%%Creator: Eitan Marder-Eppstein (Willow Garage)\n");
- printf("%%%%EndComments\n");
-}
-
-void printPSFooter(){
- printf("showpage\n%%%%EOF\n");
-}
-
-void printPolygonPS(const vector<geometry_msgs::Point>& poly, double line_width){
- if(poly.size() < 2)
- return;
-
- printf("%.2f setlinewidth\n", line_width);
- printf("newpath\n");
- printf("%.4f\t%.4f\tmoveto\n", poly[0].x * 10, poly[0].y * 10);
- for(unsigned int i = 1; i < poly.size(); ++i)
- printf("%.4f\t%.4f\tlineto\n", poly[i].x * 10, poly[i].y * 10);
- printf("%.4f\t%.4f\tlineto\n", poly[0].x * 10, poly[0].y * 10);
- printf("closepath stroke\n");
-
-}
-
-namespace iri_ackermann_local_planner {
-
-PointGrid::PointGrid(double size_x, double size_y, double resolution, geometry_msgs::Point origin, double max_z, double obstacle_range, double min_seperation) :
- resolution_(resolution), origin_(origin), max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range), sq_min_separation_(min_seperation * min_seperation)
- {
- width_ = (int) (size_x / resolution_);
- height_ = (int) (size_y / resolution_);
- cells_.resize(width_ * height_);
- }
-
- double PointGrid::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius){
- //the half-width of the circumscribed sqaure of the robot is equal to the circumscribed radius
- double outer_square_radius = circumscribed_radius;
-
- //get all the points inside the circumscribed square of the robot footprint
- geometry_msgs::Point c_lower_left, c_upper_right;
- c_lower_left.x = position.x - outer_square_radius;
- c_lower_left.y = position.y - outer_square_radius;
-
- c_upper_right.x = position.x + outer_square_radius;
- c_upper_right.y = position.y + outer_square_radius;
-
- //This may return points that are still outside of the cirumscribed square because it returns the cells
- //contained by the range
- getPointsInRange(c_lower_left, c_upper_right, points_);
-
- //if there are no points in the circumscribed square... we don't have to check against the footprint
- if(points_.empty())
- return 1.0;
-
- //compute the half-width of the inner square from the inscribed radius of the robot
- double inner_square_radius = sqrt((inscribed_radius * inscribed_radius) / 2.0);
-
- //we'll also check against the inscribed square
- geometry_msgs::Point i_lower_left, i_upper_right;
- i_lower_left.x = position.x - inner_square_radius;
- i_lower_left.y = position.y - inner_square_radius;
-
- i_upper_right.x = position.x + inner_square_radius;
- i_upper_right.y = position.y + inner_square_radius;
-
- //if there are points, we have to do a more expensive check
- for(unsigned int i = 0; i < points_.size(); ++i){
- list<pcl::PointXYZ>* cell_points = points_[i];
- if(cell_points != NULL){
- for(list<pcl::PointXYZ>::iterator it = cell_points->begin(); it != cell_points->end(); ++it){
- const pcl::PointXYZ& pt = *it;
- //first, we'll check to make sure we're in the outer square
- //printf("(%.2f, %.2f) ... l(%.2f, %.2f) ... u(%.2f, %.2f)\n", pt.x, pt.y, c_lower_left.x, c_lower_left.y, c_upper_right.x, c_upper_right.y);
- if(pt.x > c_lower_left.x && pt.x < c_upper_right.x && pt.y > c_lower_left.y && pt.y < c_upper_right.y){
- //do a quick check to see if the point lies in the inner square of the robot
- if(pt.x > i_lower_left.x && pt.x < i_upper_right.x && pt.y > i_lower_left.y && pt.y < i_upper_right.y)
- return -1.0;
-
- //now we really have to do a full footprint check on the point
- if(ptInPolygon(pt, footprint))
- return -1.0;
- }
- }
- }
- }
-
- //if we get through all the points and none of them are in the footprint it's legal
- return 1.0;
- }
-
- bool PointGrid::ptInPolygon(const pcl::PointXYZ& pt, const vector<geometry_msgs::Point>& poly){
- if(poly.size() < 3)
- return false;
-
- //a point is in a polygon iff the orientation of the point
- //with respect to sides of the polygon is the same for every
- //side of the polygon
- bool all_left = false;
- bool all_right = false;
- for(unsigned int i = 0; i < poly.size() - 1; ++i){
- //if pt left of a->b
- if(orient(poly[i], poly[i + 1], pt) > 0){
- if(all_right)
- return false;
- all_left = true;
- }
- //if pt right of a->b
- else{
- if(all_left)
- return false;
- all_right = true;
- }
- }
- //also need to check the last point with the first point
- if(orient(poly[poly.size() - 1], poly[0], pt) > 0){
- if(all_right)
- return false;
- }
- else{
- if(all_left)
- return false;
- }
-
- return true;
- }
-
- void PointGrid::getPointsInRange(const geometry_msgs::Point& lower_left, const geometry_msgs::Point& upper_right, vector< list<pcl::PointXYZ>* >& points){
- points.clear();
-
- //compute the other corners of the box so we can get cells indicies for them
- geometry_msgs::Point upper_left, lower_right;
- upper_left.x = lower_left.x;
- upper_left.y = upper_right.y;
- lower_right.x = upper_right.x;
- lower_right.y = lower_left.y;
-
- //get the grid coordinates of the cells matching the corners of the range
- unsigned int gx, gy;
-
- //if the grid coordinates are outside the bounds of the grid... return
- if(!gridCoords(lower_left, gx, gy))
- return;
- //get the associated index
- unsigned int lower_left_index = gridIndex(gx, gy);
-
- if(!gridCoords(lower_right, gx, gy))
- return;
- unsigned int lower_right_index = gridIndex(gx, gy);
-
- if(!gridCoords(upper_left, gx, gy))
- return;
- unsigned int upper_left_index = gridIndex(gx, gy);
-
- //compute x_steps and y_steps
- unsigned int x_steps = lower_right_index - lower_left_index + 1;
- unsigned int y_steps = (upper_left_index - lower_left_index) / width_ + 1;
- /*
- * (0, 0) ---------------------- (width, 0)
- * | |
- * | |
- * | |
- * | |
- * | |
- * (0, height) ----------------- (width, height)
- */
- //get an iterator
- vector< list<pcl::PointXYZ> >::iterator cell_iterator = cells_.begin() + lower_left_index;
- //printf("Index: %d, Width: %d, x_steps: %d, y_steps: %d\n", lower_left_index, width_, x_steps, y_steps);
- for(unsigned int i = 0; i < y_steps; ++i){
- for(unsigned int j = 0; j < x_steps; ++j){
- list<pcl::PointXYZ>& cell = *cell_iterator;
- //if the cell contains any points... we need to push them back to our list
- if(!cell.empty()){
- points.push_back(&cell);
- }
- if(j < x_steps - 1)
- cell_iterator++; //move a cell in the x direction
- }
- cell_iterator += width_ - (x_steps - 1); //move down a row
- }
- }
-
- void PointGrid::insert(pcl::PointXYZ pt){
- //get the grid coordinates of the point
- unsigned int gx, gy;
-
- //if the grid coordinates are outside the bounds of the grid... return
- if(!gridCoords(pt, gx, gy))
- return;
-
- //if the point is too close to its nearest neighbor... return
- if(nearestNeighborDistance(pt) < sq_min_separation_)
- return;
-
- //get the associated index
- unsigned int pt_index = gridIndex(gx, gy);
-
- //insert the point into the grid at the correct location
- cells_[pt_index].push_back(pt);
- //printf("Index: %d, size: %d\n", pt_index, cells_[pt_index].size());
- }
-
- double PointGrid::getNearestInCell(pcl::PointXYZ& pt, unsigned int gx, unsigned int gy){
- unsigned int index = gridIndex(gx, gy);
- double min_sq_dist = DBL_MAX;
- //loop through the points in the cell and find the minimum distance to the passed point
- for(list<pcl::PointXYZ>::iterator it = cells_[index].begin(); it != cells_[index].end(); ++it){
- min_sq_dist = min(min_sq_dist, sq_distance(pt, *it));
- }
- return min_sq_dist;
- }
-
-
- double PointGrid::nearestNeighborDistance(pcl::PointXYZ& pt){
- //get the grid coordinates of the point
- unsigned int gx, gy;
-
- gridCoords(pt, gx, gy);
-
- //get the bounds of the grid cell in world coords
- geometry_msgs::Point lower_left, upper_right;
- getCellBounds(gx, gy, lower_left, upper_right);
-
- //now we need to check what cells could contain the nearest neighbor
- pcl::PointXYZ check_point;
- double sq_dist = DBL_MAX;
- double neighbor_sq_dist = DBL_MAX;
-
- //left
- if(gx > 0){
- check_point.x = lower_left.x;
- check_point.y = pt.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy));
- }
-
- //upper left
- if(gx > 0 && gy < height_ - 1){
- check_point.x = lower_left.x;
- check_point.y = upper_right.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy + 1));
- }
-
- //top
- if(gy < height_ - 1){
- check_point.x = pt.x;
- check_point.y = upper_right.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy + 1));
- }
-
- //upper right
- if(gx < width_ - 1 && gy < height_ - 1){
- check_point.x = upper_right.x;
- check_point.y = upper_right.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy + 1));
- }
-
- //right
- if(gx < width_ - 1){
- check_point.x = upper_right.x;
- check_point.y = pt.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy));
- }
-
- //lower right
- if(gx < width_ - 1 && gy > 0){
- check_point.x = upper_right.x;
- check_point.y = lower_left.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx + 1, gy - 1));
- }
-
- //bottom
- if(gy > 0){
- check_point.x = pt.x;
- check_point.y = lower_left.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy - 1));
- }
-
- //lower left
- if(gx > 0 && gy > 0){
- check_point.x = lower_left.x;
- check_point.y = lower_left.y;
- sq_dist = sq_distance(pt, check_point);
- if(sq_dist < sq_min_separation_)
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx - 1, gy - 1));
- }
-
- //we must also check within the cell we're in for a nearest neighbor
- neighbor_sq_dist = min(neighbor_sq_dist, getNearestInCell(pt, gx, gy));
-
- return neighbor_sq_dist;
- }
-
- void PointGrid::updateWorld(const vector<geometry_msgs::Point>& footprint,
- const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
- //for our 2D point grid we only remove freespace based on the first laser scan
- if(laser_scans.empty())
- return;
-
- removePointsInScanBoundry(laser_scans[0]);
-
- //iterate through all observations and update the grid
- for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
- const Observation& obs = *it;
- const pcl::PointCloud<pcl::PointXYZ>& cloud = (obs.cloud_);
- for(unsigned int i = 0; i < cloud.points.size(); ++i){
- //filter out points that are too high
- if(cloud.points[i].z > max_z_)
- continue;
-
- //compute the squared distance from the hitpoint to the pointcloud's origin
- double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
- + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
- + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
-
- if(sq_dist >= sq_obstacle_range_)
- continue;
-
- //insert the point
- insert(cloud.points[i]);
- }
- }
-
- //remove the points that are in the footprint of the robot
- removePointsInPolygon(footprint);
- }
-
- void PointGrid::removePointsInScanBoundry(const PlanarLaserScan& laser_scan){
- if(laser_scan.cloud.points.size() == 0)
- return;
-
- //compute the containing square of the scan
- geometry_msgs::Point lower_left, upper_right;
- lower_left.x = laser_scan.origin.x;
- lower_left.y = laser_scan.origin.y;
- upper_right.x = laser_scan.origin.x;
- upper_right.y = laser_scan.origin.y;
-
- for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
- lower_left.x = min(lower_left.x, (double)laser_scan.cloud.points[i].x);
- lower_left.y = min(lower_left.y, (double)laser_scan.cloud.points[i].y);
- upper_right.x = max(upper_right.x, (double)laser_scan.cloud.points[i].x);
- upper_right.y = max(upper_right.y, (double)laser_scan.cloud.points[i].y);
- }
-
- getPointsInRange(lower_left, upper_right, points_);
-
- //if there are no points in the containing square... we don't have to do anything
- if(points_.empty())
- return;
-
- //if there are points, we have to check them against the scan explicitly to remove them
- for(unsigned int i = 0; i < points_.size(); ++i){
- list<pcl::PointXYZ>* cell_points = points_[i];
- if(cell_points != NULL){
- list<pcl::PointXYZ>::iterator it = cell_points->begin();
- while(it != cell_points->end()){
- const pcl::PointXYZ& pt = *it;
-
- //check if the point is in the polygon and if it is, erase it from the grid
- if(ptInScan(pt, laser_scan)){
- it = cell_points->erase(it);
- }
- else
- it++;
- }
- }
- }
- }
-
- bool PointGrid::ptInScan(const pcl::PointXYZ& pt, const PlanarLaserScan& laser_scan){
- if(!laser_scan.cloud.points.empty()){
- //compute the angle of the point relative to that of the scan
- double v1_x = laser_scan.cloud.points[0].x - laser_scan.origin.x;
- double v1_y = laser_scan.cloud.points[0].y - laser_scan.origin.y;
- double v2_x = pt.x - laser_scan.origin.x;
- double v2_y = pt.y - laser_scan.origin.y;
-
- double perp_dot = v1_x * v2_y - v1_y * v2_x;
- double dot = v1_x * v2_x + v1_y * v2_y;
-
- //get the signed angle
- double vector_angle = atan2(perp_dot, dot);
-
- //we want all angles to be between 0 and 2PI
- if(vector_angle < 0)
- vector_angle = 2 * M_PI + vector_angle;
-
- double total_rads = laser_scan.angle_max - laser_scan.angle_min;
-
- //if this point lies outside of the scan field of view... it is not in the scan
- if(vector_angle < 0 || vector_angle >= total_rads)
- return false;
-
- //compute the index of the point in the scan
- unsigned int index = (unsigned int) (vector_angle / laser_scan.angle_increment);
-
- //make sure we have a legal index... we always should at this point, but just in case
- if(index >= laser_scan.cloud.points.size() - 1){
- return false;
- }
-
- //if the point lies to the left of the line between the two scan points bounding it, it is within the scan
- if(orient(laser_scan.cloud.points[index], laser_scan.cloud.points[index + 1], pt) > 0){
- return true;
- }
-
- //otherwise it is not
- return false;
- }
- else
- return false;
- }
-
- void PointGrid::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
- for(unsigned int i = 0; i < cells_.size(); ++i){
- for(list<pcl::PointXYZ>::iterator it = cells_[i].begin(); it != cells_[i].end(); ++it){
- cloud.points.push_back(*it);
- }
- }
- }
-
- void PointGrid::removePointsInPolygon(const vector<geometry_msgs::Point> poly){
- if(poly.size() == 0)
- return;
-
- geometry_msgs::Point lower_left, upper_right;
- lower_left.x = poly[0].x;
- lower_left.y = poly[0].y;
- upper_right.x = poly[0].x;
- upper_right.y = poly[0].y;
-
- //compute the containing square of the polygon
- for(unsigned int i = 1; i < poly.size(); ++i){
- lower_left.x = min(lower_left.x, poly[i].x);
- lower_left.y = min(lower_left.y, poly[i].y);
- upper_right.x = max(upper_right.x, poly[i].x);
- upper_right.y = max(upper_right.y, poly[i].y);
- }
-
- ROS_DEBUG("Lower: (%.2f, %.2f), Upper: (%.2f, %.2f)\n", lower_left.x, lower_left.y, upper_right.x, upper_right.y);
- getPointsInRange(lower_left, upper_right, points_);
-
- //if there are no points in the containing square... we don't have to do anything
- if(points_.empty())
- return;
-
- //if there are points, we have to check them against the polygon explicitly to remove them
- for(unsigned int i = 0; i < points_.size(); ++i){
- list<pcl::PointXYZ>* cell_points = points_[i];
- if(cell_points != NULL){
- list<pcl::PointXYZ>::iterator it = cell_points->begin();
- while(it != cell_points->end()){
- const pcl::PointXYZ& pt = *it;
-
- //check if the point is in the polygon and if it is, erase it from the grid
- if(ptInPolygon(pt, poly)){
- it = cell_points->erase(it);
- }
- else
- it++;
- }
- }
- }
- }
-
- void PointGrid::intersectionPoint(const geometry_msgs::Point& v1, const geometry_msgs::Point& v2,
- const geometry_msgs::Point& u1, const geometry_msgs::Point& u2, geometry_msgs::Point& result){
- //generate the equation for line 1
- double a1 = v2.y - v1.y;
- double b1 = v1.x - v2.x;
- double c1 = a1 * v1.x + b1 * v1.y;
-
- //generate the equation for line 2
- double a2 = u2.y - u1.y;
- double b2 = u1.x - u2.x;
- double c2 = a2 * u1.x + b2 * u1.y;
-
- double det = a1 * b2 - a2 * b1;
-
- //the lines are parallel
- if(det == 0)
- return;
-
- result.x = (b2 * c1 - b1 * c2) / det;
- result.y = (a1 * c2 - a2 * c1) / det;
- }
-
-};
-
-
-using namespace iri_ackermann_local_planner;
-
-int main(int argc, char** argv){
- geometry_msgs::Point origin;
- origin.x = 0.0;
- origin.y = 0.0;
- PointGrid pg(50.0, 50.0, 0.2, origin, 2.0, 3.0, 0.0);
- /*
- double x = 10.0;
- double y = 10.0;
- for(int i = 0; i < 100; ++i){
- for(int j = 0; j < 100; ++j){
- pcl::PointXYZ pt;
- pt.x = x;
- pt.y = y;
- pt.z = 1.0;
- pg.insert(pt);
- x += .03;
- }
- y += .03;
- x = 10.0;
- }
- */
- vector<geometry_msgs::Point> footprint;
- geometry_msgs::Point pt;
-
- pt.x = 1.0;
- pt.y = 1.0;
- footprint.push_back(pt);
-
- pt.x = 1.0;
- pt.y = 1.65;
- footprint.push_back(pt);
-
- pt.x = 1.325;
- pt.y = 1.75;
- footprint.push_back(pt);
-
- pt.x = 1.65;
- pt.y = 1.65;
- footprint.push_back(pt);
-
- pt.x = 1.65;
- pt.y = 1.0;
- footprint.push_back(pt);
-
- vector<geometry_msgs::Point> footprint2;
-
- pt.x = 1.325;
- pt.y = 1.00;
- footprint2.push_back(pt);
-
- pt.x = 1.325;
- pt.y = 1.75;
- footprint2.push_back(pt);
-
- pt.x = 1.65;
- pt.y = 1.75;
- footprint2.push_back(pt);
-
- pt.x = 1.65;
- pt.y = 1.00;
- footprint2.push_back(pt);
-
- vector<geometry_msgs::Point> footprint3;
-
- pt.x = 0.99;
- pt.y = 0.99;
- footprint3.push_back(pt);
-
- pt.x = 0.99;
- pt.y = 1.66;
- footprint3.push_back(pt);
-
- pt.x = 1.3255;
- pt.y = 1.85;
- footprint3.push_back(pt);
-
- pt.x = 1.66;
- pt.y = 1.66;
- footprint3.push_back(pt);
-
- pt.x = 1.66;
- pt.y = 0.99;
- footprint3.push_back(pt);
-
- pt.x = 1.325;
- pt.y = 1.325;
-
- pcl::PointXYZ point;
- point.x = 1.2;
- point.y = 1.2;
- point.z = 1.0;
-
- struct timeval start, end;
- double start_t, end_t, t_diff;
-
- printPSHeader();
-
- gettimeofday(&start, NULL);
- for(unsigned int i = 0; i < 2000; ++i){
- pg.insert(point);
- }
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
- printf("%%Insertion Time: %.9f \n", t_diff);
-
- vector<Observation> obs;
- vector<PlanarLaserScan> scan;
-
- gettimeofday(&start, NULL);
- pg.updateWorld(footprint, obs, scan);
- double legal = pg.footprintCost(pt, footprint, 0.0, .95);
- pg.updateWorld(footprint, obs, scan);
- double legal2 = pg.footprintCost(pt, footprint, 0.0, .95);
- gettimeofday(&end, NULL);
- start_t = start.tv_sec + double(start.tv_usec) / 1e6;
- end_t = end.tv_sec + double(end.tv_usec) / 1e6;
- t_diff = end_t - start_t;
-
- printf("%%Footprint calc: %.9f \n", t_diff);
-
- if(legal >= 0.0)
- printf("%%Legal footprint %.4f, %.4f\n", legal, legal2);
- else
- printf("%%Illegal footprint\n");
-
- printPSFooter();
-
- return 0;
-}
diff --git a/src/trajectory.cpp b/src/trajectory.cpp
deleted file mode 100755
index 75825c7ccb722d6bbb98b3387cbae1006249f8e6..0000000000000000000000000000000000000000
--- a/src/trajectory.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above
- * copyright notice, this list of conditions and the following
- * disclaimer in the documentation and/or other materials provided
- * with the distribution.
- * * Neither the name of the Willow Garage nor the names of its
- * contributors may be used to endorse or promote products derived
- * from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-#include <trajectory.h>
-
-namespace iri_ackermann_local_planner {
- Trajectory::Trajectory()
- : xv_(0.0), yv_(0.0), thetav_(0.0), cost_(-1.0)
- {
- }
-
- Trajectory::Trajectory(double xv, double yv, double thetav, unsigned int num_pts)
- : xv_(xv), yv_(yv), thetav_(thetav), cost_(-1.0), x_pts_(num_pts), y_pts_(num_pts), th_pts_(num_pts)
- {
- }
-
- void Trajectory::getPoint(unsigned int index, double& x, double& y, double& th){
- x = x_pts_[index];
- y = y_pts_[index];
- th = th_pts_[index];
- }
-
- void Trajectory::setPoint(unsigned int index, double x, double y, double th){
- x_pts_[index] = x;
- y_pts_[index] = y;
- th_pts_[index] = th;
- }
-
- void Trajectory::addPoint(double x, double y, double th){
- x_pts_.push_back(x);
- y_pts_.push_back(y);
- th_pts_.push_back(th);
- }
-
- void Trajectory::resetPoints(){
- x_pts_.clear();
- y_pts_.clear();
- th_pts_.clear();
- }
-
- void Trajectory::getEndpoint(double& x, double& y, double& th){
- x = x_pts_.back();
- y = y_pts_.back();
- th = th_pts_.back();
- }
-
- unsigned int Trajectory::getPointsSize(){
- return x_pts_.size();
- }
-};
diff --git a/src/trajectory_planner.cpp b/src/trajectory_planner.cpp
deleted file mode 100755
index 5850bdfbcd72b82279ab4438dad190a483095b75..0000000000000000000000000000000000000000
--- a/src/trajectory_planner.cpp
+++ /dev/null
@@ -1,1266 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-*********************************************************************/
-
-#include <trajectory_planner.h>
-
-using namespace std;
-using namespace costmap_2d;
-
-namespace iri_ackermann_local_planner{
- void TrajectoryPlanner::reconfigure(AckermannLocalPlannerConfig &cfg)
- {
- iri_ackermann_local_planner::AckermannLocalPlannerConfig config(cfg);
- boost::mutex::scoped_lock l(configuration_mutex_);
-
- sim_time_ = config.sim_time;
- sim_granularity_ = config.sim_granularity;
-
- pdist_scale_ = config.pdist_scale;
- gdist_scale_ = config.gdist_scale;
- hdiff_scale_ = config.hdiff_scale;
- occdist_scale_ = config.occdist_scale;
-
- vx_samples_ = config.vx_samples;
- vtheta_samples_ = config.vtheta_samples;
-
- if (vx_samples_ <= 0)
- {
- config.vx_samples = 1;
- vx_samples_ = config.vx_samples;
- ROS_WARN("You've specified that you don't want any samples in the x dimension. We'll at least assume that you want to sample one value... so we're going to set vx_samples to 1 instead");
- }
- if(vtheta_samples_ <= 0)
- {
- config.vtheta_samples = 1;
- vtheta_samples_ = config.vtheta_samples;
- ROS_WARN("You've specified that you don't want any samples in the theta dimension. We'll at least assume that you want to sample one value... so we're going to set vtheta_samples to 1 instead");
- }
-
- simple_attractor_ = config.simple_attractor;
-
- angular_sim_granularity_ = config.angular_sim_granularity;
-
- /* ackerman parameters */
- this->ack_vel_max_=config.ack_vel_max;//config.ack_vel_max;
- this->ack_vel_min_=config.ack_vel_max;//config.ack_vel_min;
- this->ack_acc_max_=config.ack_acc_max;//config.ack_acc_max;
- this->ack_steer_angle_max_=config.ack_steer_angle_max;//config.ack_steer_angle_max;
- this->ack_steer_angle_min_=config.ack_steer_angle_min;//config.ack_steer_angle_min;
- this->ack_steer_speed_max_=config.ack_steer_speed_max;//config.ack_steer_speed_max;
- this->ack_steer_speed_min_=config.ack_steer_speed_min;//config.ack_steer_speed_min;
- this->ack_steer_acc_max_=config.ack_steer_acc_max;//config.ack_steer_acc_max;
- this->ack_axis_distance_=config.ack_axis_distance;
- this->heading_points_=config.heading_points;
- }
-
- TrajectoryPlanner::TrajectoryPlanner(WorldModel &world_model,
- const costmap_2d::Costmap2D& costmap,
- std::vector<geometry_msgs::Point> footprint_spec,
- double max_acc, double max_vel, double min_vel,
- double max_steer_acc, double max_steer_vel, double min_steer_vel,
- double max_steer_angle, double min_steer_angle,double axis_distance,
- double sim_time, double sim_granularity,
- int vx_samples, int vtheta_samples,
- double pdist_scale, double gdist_scale, double occdist_scale, double hdiff_scale,
- bool simple_attractor, double angular_sim_granularity,int heading_points,double xy_goal_tol)
- : map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()), costmap_(costmap),
- world_model_(world_model), footprint_spec_(footprint_spec),
- sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
- vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
- pdist_scale_(pdist_scale), gdist_scale_(gdist_scale), occdist_scale_(occdist_scale),hdiff_scale_(hdiff_scale),
- ack_acc_max_(max_acc), ack_vel_min_(min_vel), ack_vel_max_(max_vel),
- ack_steer_acc_max_(max_steer_acc),ack_steer_speed_max_(max_steer_vel),ack_steer_speed_min_(min_steer_vel),
- ack_steer_angle_max_(max_steer_angle),ack_steer_angle_min_(min_steer_angle),ack_axis_distance_(axis_distance),
- simple_attractor_(simple_attractor),heading_points_(heading_points),xy_goal_tol_(xy_goal_tol)
- {
- }
-
- TrajectoryPlanner::~TrajectoryPlanner(){}
-
- bool TrajectoryPlanner::getCellCosts(int cx, int cy, float &path_cost, float &goal_cost, float &occ_cost, float &total_cost) {
- MapCell cell = map_(cx, cy);
- if (cell.within_robot) {
- return false;
- }
- occ_cost = costmap_.getCost(cx, cy);
- if (cell.path_dist >= map_.map_.size() || cell.goal_dist >= map_.map_.size() || occ_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) {
- return false;
- }
- path_cost = cell.path_dist;
- goal_cost = cell.goal_dist;
- total_cost = pdist_scale_ * path_cost + gdist_scale_ * goal_cost + occdist_scale_ * occ_cost;
- return true;
- }
-
- //create and score a trajectory given the current pose of the robot and selected velocities
- void TrajectoryPlanner::generateTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp,
- double acc_x, double acc_y, double acc_theta, double impossible_cost,
- Trajectory& traj){
-
- // make sure the configuration doesn't change mid run
- boost::mutex::scoped_lock l(configuration_mutex_);
-
- // store the current state of the robot (pose (x,y,theta) and speed
- double x_i = x;
- double y_i = y;
- double theta_i = theta;
-
- // ackerman current state
- double vt_i;
- double steer_angle_i,steer_speed_i;
- double vx_int,vy_int;
- double r,d;
-
- // for the ackerman configuration, vx_samp -> v_t and vtheta_samp -> steer_angle
- vt_i=vx;
- steer_angle_i=vy;
- steer_speed_i=vtheta;
-
- vx_int = vx_samp;
- vy_int = 0;
-
- //compute the number of steps we must take along this trajectory to be "safe"
- int num_steps;
- num_steps = int(sim_time_ / sim_granularity_ + 0.5);
-
- //we at least want to take one step... even if we won't move, we want to score our current position
- if(num_steps == 0)
- num_steps = 1;
-
- double dt = sim_time_ / num_steps;
- double time = 0.0;
-
- //create a potential trajectory
- traj.resetPoints();
- traj.xv_ = vx_int;
- traj.yv_ = vy_int;
- traj.thetav_ = vtheta_samp;
- traj.cost_ = -1.0;
-
- //initialize the costs for the trajectory
- double path_dist = 0.0;
- double goal_dist = 0.0;
- double occ_cost = 0.0;
- double heading_diff = 0.0;
-
- double speed=0.0,angle=0.0;
- double T1=0.0,T4=0.0,T2=0.0,T3=0.0;
-
- /* check wether the trajectory can be generated or not */
- if(vtheta<0 && (-vtheta*vtheta/(2*this->ack_steer_acc_max_)+vy)>this->ack_steer_angle_max_)
- return;
-
- if(vtheta>0 && (vtheta*vtheta/(2*this->ack_steer_acc_max_)+vy)>this->ack_steer_angle_max_)
- return;
-
- if(vtheta<0 && (-vtheta*vtheta/(2*this->ack_steer_acc_max_)+vy)<this->ack_steer_angle_min_)
- return;
-
- if(vtheta>0 && (vtheta*vtheta/(2*this->ack_steer_acc_max_)+vy)<this->ack_steer_angle_min_)
- return;
-
- // compute the trajectory times
- if(steer_speed_i>=0)
- {
- if(vtheta_samp>steer_angle_i)
- {
- speed=(steer_speed_i+this->ack_steer_acc_max_*sim_time_)/2.0;
- if(speed>=this->ack_steer_speed_max_)
- {
- speed=this->ack_steer_speed_max_;
- T4=sim_time_-(2.0*speed-steer_speed_i)/this->ack_steer_acc_max_;
- angle=speed*speed/this->ack_steer_acc_max_-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+speed*sim_time_*T4+steer_angle_i;
- if(angle>vtheta_samp)
- {
- angle=vtheta_samp;
- T4=(vtheta_samp-steer_angle_i-speed*speed/this->ack_steer_acc_max_+steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_))/speed;
- if(T4<0)
- {
- T4=0;
- speed=sqrt(steer_speed_i*steer_speed_i/2.0+this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- }
- else
- {
- angle=speed*speed/this->ack_steer_acc_max_-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+steer_angle_i;
- T4=0;
- if(angle>vtheta_samp)
- {
- angle=vtheta_samp;
- speed=sqrt(steer_speed_i*steer_speed_i/2.0+this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- T1=(speed-steer_speed_i)/this->ack_steer_acc_max_;
- if(T1<0)
- return;
- }
- else
- {
- speed=(steer_speed_i-this->ack_steer_acc_max_*sim_time_)/2.0;
- if(speed<=this->ack_steer_speed_min_)
- {
- speed=this->ack_steer_speed_min_;
- T4=sim_time_+(2.0*speed-steer_speed_i)/this->ack_steer_acc_max_;
- angle=steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)-speed*speed/this->ack_steer_acc_max_+speed*T4+steer_angle_i;
- if(angle<vtheta_samp)
- {
- angle=vtheta_samp;
- T4=(vtheta_samp-steer_angle_i+speed*speed/this->ack_steer_acc_max_-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_))/speed;
- if(T4<0)
- {
- T4=0;
- speed=-sqrt(steer_speed_i*steer_speed_i/2.0-this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- }
- else
- {
- angle=steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)-speed*speed/this->ack_steer_acc_max_+steer_angle_i;
- T4=0;
- if(angle<vtheta_samp)
- {
- angle=vtheta_samp;
- speed=-sqrt(steer_speed_i*steer_speed_i/2.0-this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- T1=-(speed-steer_speed_i)/this->ack_steer_acc_max_;
- if(T1<0)
- return;
- }
- }
- else
- {
- if(vtheta_samp>steer_angle_i)
- {
- speed=(steer_speed_i+this->ack_steer_acc_max_*sim_time_)/2.0;
- if(speed>=this->ack_steer_speed_max_)
- {
- speed=this->ack_steer_speed_max_;
- T4=sim_time_-(2.0*speed-steer_speed_i)/this->ack_steer_acc_max_;
- angle=-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+speed*speed/this->ack_steer_acc_max_+speed*T4+steer_angle_i;
- if(angle>vtheta_samp)
- {
- angle=vtheta_samp;
- T4=(vtheta_samp-steer_angle_i-speed*speed/this->ack_steer_acc_max_+steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_))/speed;
- if(T4<0)
- {
- T4=0;
- speed=sqrt(steer_speed_i*steer_speed_i/2.0+this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- }
- else
- {
- angle=-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+speed*speed/this->ack_steer_acc_max_+steer_angle_i;
- T4=0;
- if(angle>vtheta_samp)
- {
- angle=vtheta_samp;
- speed=sqrt(steer_speed_i*steer_speed_i/2.0+this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- T1=(speed-steer_speed_i)/this->ack_steer_acc_max_;
- if(T1<0)
- return;
- }
- else
- {
- speed=(steer_speed_i-this->ack_steer_acc_max_*sim_time_)/2.0;
- if(speed<=this->ack_steer_speed_min_)
- {
- speed=this->ack_steer_speed_min_;
- T4=sim_time_+(2.0*speed-steer_speed_i)/this->ack_steer_acc_max_;
- angle=-speed*speed/this->ack_steer_acc_max_+steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+speed*T4+steer_angle_i;
- if(angle<vtheta_samp)
- {
- angle=vtheta_samp;
- T4=(vtheta_samp-steer_angle_i+speed*speed/this->ack_steer_acc_max_-steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_))/speed;
- if(T4<0)
- {
- T4=0;
- speed=-sqrt(steer_speed_i*steer_speed_i/2.0-this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- }
- else
- {
- angle=-speed*speed/this->ack_steer_acc_max_+steer_speed_i*steer_speed_i/(2.0*this->ack_steer_acc_max_)+steer_angle_i;
- T4=0;
- if(angle<vtheta_samp)
- {
- angle=vtheta_samp;
- speed=-sqrt(steer_speed_i*steer_speed_i/2.0-this->ack_steer_acc_max_*(vtheta_samp-steer_angle_i));
- }
- }
- T1=-(speed-steer_speed_i)/this->ack_steer_acc_max_;
- if(T1<0)
- return;
- }
- }
-
- double v=0.0;
- if(vx_samp>vt_i)
- {
- v=(vt_i+this->ack_acc_max_*sim_time_)/2.0;
- if(v>vx_samp)
- {
- v=vx_samp;
- T3=sim_time_-(2.0*v-vt_i)/this->ack_acc_max_;
- }
- else
- T3=0;
- T2=(v-vt_i)/this->ack_acc_max_;
- if(T2<0)
- return;
- }
- else
- {
- v=(vt_i-this->ack_acc_max_*sim_time_)/2.0;
- if(v<vx_samp)
- {
- v=vx_samp;
- T3=sim_time_+(2.0*v-vt_i)/this->ack_acc_max_;
- }
- else
- T3=0;
- T2=-(v-vt_i)/this->ack_acc_max_;
- if(T2<0)
- return;
- }
-
- double time_window_start=sim_time_-2*dt;///heading_points_;
- double time_window_end=sim_time_-dt;///heading_points_+dt;
-
- for(int i = 0; i < num_steps; ++i){
- heading_diff=3.14159;
- //get map coordinates of a point
- unsigned int cell_x, cell_y;
-
- //we don't want a path that goes off the know map
- if(!costmap_.worldToMap(x_i, y_i, cell_x, cell_y)){
- traj.cost_ = -1.0;
- ROS_DEBUG("TrajectoryPlanner::generateTrajectory: Current point out of map!");
- return;
- }
-
- //check the point on the trajectory for legality
- double footprint_cost = footprintCost(x_i, y_i, theta_i);
-
- occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
-
- double cell_pdist = map_(cell_x, cell_y).path_dist;
- double cell_gdist = map_(cell_x, cell_y).goal_dist;
-
- double near_dist=DBL_MAX,dist;
- unsigned int near_index=0;
-
- //update path and goal distances
- path_dist = cell_pdist;
- goal_dist = cell_gdist;
- if(time >= time_window_start && time < time_window_end)
- {
- // find the nearrest point on the path
- for(unsigned int i = 0;i<global_plan_.size();i++)
- {
- dist=sqrt((global_plan_[i].pose.position.x-x_i)*(global_plan_[i].pose.position.x-x_i)+(global_plan_[i].pose.position.y-y_i)*(global_plan_[i].pose.position.y-y_i));
- if(dist<near_dist)
- {
- near_dist=dist;
- near_index=i;
- }
- }
- double v1_x,v1_y;
- if(near_index==0)
- {
- v1_x = global_plan_[near_index+1].pose.position.x - global_plan_[near_index].pose.position.x;
- v1_y = global_plan_[near_index+1].pose.position.y - global_plan_[near_index].pose.position.y;
- }
- else
- {
- v1_x = global_plan_[near_index].pose.position.x - global_plan_[near_index-1].pose.position.x;
- v1_y = global_plan_[near_index].pose.position.y - global_plan_[near_index-1].pose.position.y;
- }
- double v2_x = cos(theta_i);
- double v2_y = sin(theta_i);
-
- double perp_dot = v1_x * v2_y - v1_y * v2_x;
- double dot = v1_x * v2_x + v1_y * v2_y;
-
- //get the signed angle
- heading_diff = fabs(atan2(perp_dot, dot));
- if(heading_diff>(3.14159/2.0))
- heading_diff=fabs(heading_diff-3.14159);
- ROS_INFO("TrajectoryPlanner::createTrajectories: heading_dist: %f", heading_diff);
-// time_window_start+=sim_time_/heading_points_;
-// time_window_end=time_window_start+dt;
- }
-
- //do we want to follow blindly
- if(simple_attractor_){
- goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
- (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
- (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
- (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
- path_dist = 0.0;
- }
- else{
- //if a point on this trajectory has no clear path to goal it is invalid
- if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
- ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
- cell_gdist, cell_pdist, impossible_cost);
- traj.cost_ = -2.0;
- return;
- }
- }
-
-
- //the point is legal... add it to the trajectory
- traj.addPoint(x_i, y_i, theta_i);
-
- // compute the next point in the trajectory
- if(vtheta_samp>vtheta)
- {
- if(time<T1)
- {
- steer_speed_i=steer_speed_i+this->ack_steer_acc_max_*dt;
- if(steer_speed_i>speed)
- steer_speed_i=speed;
- }
- else if(time<T1+T4)
- steer_speed_i=speed;
- else
- {
- steer_speed_i=steer_speed_i-this->ack_steer_acc_max_*dt;
- if(steer_speed_i<0)
- steer_speed_i=0;
- }
- }
- else
- {
- if(time<T1)
- {
- steer_speed_i=steer_speed_i-this->ack_steer_acc_max_*dt;
- if(steer_speed_i<speed)
- steer_speed_i=speed;
- }
- else if(time<T1+T4)
- steer_speed_i=speed;
- else
- {
- steer_speed_i=steer_speed_i+this->ack_steer_acc_max_*dt;
- if(steer_speed_i>0)
- steer_speed_i=0;
- }
- }
- steer_angle_i+=steer_speed_i*dt;
- if(vx_samp>vx)
- {
- if(time<T2)
- {
- vt_i=vt_i+this->ack_acc_max_*dt;
- if(vt_i>v)
- vt_i=v;
- }
- else if(time<(T2+T3))
- vt_i=v;
- else
- {
- vt_i=vt_i-this->ack_acc_max_*dt;
- if(vt_i<0)
- vt_i=0;
- }
- }
- else
- {
- if(time<T2)
- {
- vt_i=vt_i-this->ack_acc_max_*dt;
- if(vt_i<v)
- vt_i=v;
- }
- else if(time<T2+T3)
- vt_i=v;
- else
- {
- vt_i=vt_i+this->ack_acc_max_*dt;
- if(vt_i>0)
- vt_i=0;
- }
- }
-
- if(fabs(steer_angle_i)>0.02)
- {
- r=ack_axis_distance_*tan(3.14159/2.0-steer_angle_i);
- d=vt_i*dt;
- x_i+=d*cos(theta_i);
- y_i+=d*sin(theta_i);
- theta_i+=d/r;
- }
- else
- {
- d=vt_i*dt;
- x_i+=d*cos(theta_i);
- y_i+=d*sin(theta_i);
- }
-
- //increment time
- time += dt;
- }
-
- //ROS_INFO("OccCost: %f, vx: %.2f, vy: %.2f, vtheta: %.2f", occ_cost, vx_samp, vy_samp, vtheta_samp);
- double cost = -1.0;
- cost = (pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost + hdiff_scale_ * heading_diff)*(1.0+0.0*fabs(vy-vtheta_samp));
-
- traj.cost_ = cost;
- }
-
- double TrajectoryPlanner::headingDiff(int cell_x, int cell_y, double x, double y, double heading){
- double heading_diff = DBL_MAX;
- unsigned int goal_cell_x, goal_cell_y;
- //find a clear line of sight from the robot's cell to a point on the path
- for(int i = global_plan_.size() - 1; i >=0; --i){
- if(costmap_.worldToMap(global_plan_[i].pose.position.x, global_plan_[i].pose.position.y, goal_cell_x, goal_cell_y)){
- if(lineCost(cell_x, goal_cell_x, cell_y, goal_cell_y) >= 0){
- double gx, gy;
- costmap_.mapToWorld(goal_cell_x, goal_cell_y, gx, gy);
- double v1_x = gx - x;
- double v1_y = gy - y;
- double v2_x = cos(heading);
- double v2_y = sin(heading);
-
- double perp_dot = v1_x * v2_y - v1_y * v2_x;
- double dot = v1_x * v2_x + v1_y * v2_y;
-
- //get the signed angle
- double vector_angle = atan2(perp_dot, dot);
-
- heading_diff = fabs(vector_angle);
- return heading_diff;
-
- }
- }
- }
- return heading_diff;
- }
-
- //calculate the cost of a ray-traced line
- double TrajectoryPlanner::lineCost(int x0, int x1,
- int y0, int y1){
- //Bresenham Ray-Tracing
- int deltax = abs(x1 - x0); // The difference between the x's
- int deltay = abs(y1 - y0); // The difference between the y's
- int x = x0; // Start x off at the first pixel
- int y = y0; // Start y off at the first pixel
-
- int xinc1, xinc2, yinc1, yinc2;
- int den, num, numadd, numpixels;
-
- double line_cost = 0.0;
- double point_cost = -1.0;
-
- if (x1 >= x0) // The x-values are increasing
- {
- xinc1 = 1;
- xinc2 = 1;
- }
- else // The x-values are decreasing
- {
- xinc1 = -1;
- xinc2 = -1;
- }
-
- if (y1 >= y0) // The y-values are increasing
- {
- yinc1 = 1;
- yinc2 = 1;
- }
- else // The y-values are decreasing
- {
- yinc1 = -1;
- yinc2 = -1;
- }
-
- if (deltax >= deltay) // There is at least one x-value for every y-value
- {
- xinc1 = 0; // Don't change the x when numerator >= denominator
- yinc2 = 0; // Don't change the y for every iteration
- den = deltax;
- num = deltax / 2;
- numadd = deltay;
- numpixels = deltax; // There are more x-values than y-values
- }
- else // There is at least one y-value for every x-value
- {
- xinc2 = 0; // Don't change the x for every iteration
- yinc1 = 0; // Don't change the y when numerator >= denominator
- den = deltay;
- num = deltay / 2;
- numadd = deltax;
- numpixels = deltay; // There are more y-values than x-values
- }
-
- for (int curpixel = 0; curpixel <= numpixels; curpixel++)
- {
- point_cost = pointCost(x, y); //Score the current point
-
- if(point_cost < 0)
- return -1;
-
- if(line_cost < point_cost)
- line_cost = point_cost;
-
- num += numadd; // Increase the numerator by the top of the fraction
- if (num >= den) // Check if numerator >= denominator
- {
- num -= den; // Calculate the new numerator value
- x += xinc1; // Change the x as appropriate
- y += yinc1; // Change the y as appropriate
- }
- x += xinc2; // Change the x as appropriate
- y += yinc2; // Change the y as appropriate
- }
-
- return line_cost;
- }
-
- double TrajectoryPlanner::pointCost(int x, int y){
- unsigned char cost = costmap_.getCost(x, y);
- //if the cell is in an obstacle the path is invalid
- if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE || cost == NO_INFORMATION){
- return -1;
- }
-
- return cost;
- }
-
- void TrajectoryPlanner::updatePlan(const vector<geometry_msgs::PoseStamped>& new_plan, bool compute_dists)
- {
- global_plan_.resize(new_plan.size());
- for(unsigned int i = 0; i < new_plan.size(); ++i)
- {
- global_plan_[i] = new_plan[i];
- }
- if(compute_dists){
- //reset the map for new operations
- map_.resetPathDist();
- //make sure that we update our path based on the global plan and compute costs
- map_.setPathCells(costmap_, global_plan_);
- ROS_DEBUG("Path/Goal distance computed");
- }
- }
-
- bool TrajectoryPlanner::checkTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
- Trajectory t;
-
- double cost = scoreTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp);
-
- //if the trajectory is a legal one... the check passes
- if(cost >= 0)
- return true;
-
- //otherwise the check fails
- return false;
- }
-
- double TrajectoryPlanner::scoreTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp){
- Trajectory t;
- double impossible_cost = map_.map_.size();
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
- ack_acc_max_, ack_vel_max_, ack_vel_min_, impossible_cost, t);
-
- // return the cost.
- return double( t.cost_ );
- }
-
- //create the trajectories we wish to score
- Trajectory TrajectoryPlanner::createTrajectories(double x, double y, double theta,
- double vx, double vy, double vtheta,
- double acc_x, double acc_y, double acc_theta){
- //compute feasible velocity limits in robot space
- double max_vel_x, max_vel_theta;
- double min_vel_x, min_vel_theta;
- // ackerman variables
- double v_t;
- double steer_angle;
- double min_steer=0,max_steer=0;
-
- double dvx;
- double dvtheta;
-
- // should we use the ackerman configuration?
- /* transform the general status to the ackerman status */
- v_t=vx;
- if(vy>this->ack_steer_angle_max_)
- vy=this->ack_steer_angle_max_;
- else if(vy<this->ack_steer_angle_min_)
- vy=this->ack_steer_angle_min_;
- steer_angle=vy;
- if(vtheta>this->ack_steer_speed_max_)
- vtheta=this->ack_steer_speed_max_;
- else if(vtheta<this->ack_steer_speed_min_)
- vtheta=this->ack_steer_speed_min_;
- this->steering_speed_=vtheta;
- /* compute the margins */
- //should we use the dynamic window approach?
- double T4=0.0;
-
- // compute the simulation time
- double dist=sqrt((global_plan_[global_plan_.size()-1].pose.position.x-x)*(global_plan_[global_plan_.size()-1].pose.position.x-x)+
- (global_plan_[global_plan_.size()-1].pose.position.y-y)*(global_plan_[global_plan_.size()-1].pose.position.y-y));
- sim_time_=dist/ack_vel_max_;
- if(sim_time_>10.0)
- sim_time_=10.0;
- else if(sim_time_<3.0)
- sim_time_=3.0;
- ROS_WARN("Simulation time %f\n",sim_time_);
- // compute the trajectory times
- if(this->steering_speed_>=0)
- {
- max_vel_theta=(this->steering_speed_+this->ack_steer_acc_max_*sim_time_)/2.0;
- if(max_vel_theta>=this->ack_steer_speed_max_)
- {
- max_vel_theta=this->ack_steer_speed_max_;
- T4=sim_time_-(2.0*max_vel_theta-this->steering_speed_)/this->ack_steer_acc_max_;
- max_steer=max_vel_theta*max_vel_theta/this->ack_steer_acc_max_-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+max_vel_theta*T4+steer_angle;
- if(max_steer>this->ack_steer_angle_max_)
- {
- max_steer=this->ack_steer_angle_max_;
- T4=(this->ack_steer_angle_max_-steer_angle-max_vel_theta*max_vel_theta/this->ack_steer_acc_max_+this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_))/max_vel_theta;
- if(T4<0)
- {
- T4=0;
- max_vel_theta=sqrt(this->steering_speed_*this->steering_speed_/2.0+this->ack_steer_acc_max_*(this->ack_steer_angle_max_-steer_angle));
- }
- }
- }
- else
- {
- max_steer=max_vel_theta*max_vel_theta/this->ack_steer_acc_max_-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+steer_angle;
- T4=0;
- if(max_steer>this->ack_steer_angle_max_)
- {
- max_steer=this->ack_steer_angle_max_;
- max_vel_theta=sqrt(this->steering_speed_*this->steering_speed_/2.0+this->ack_steer_acc_max_*(this->ack_steer_angle_max_-steer_angle));
- }
- }
- min_vel_theta=(this->steering_speed_-this->ack_steer_acc_max_*sim_time_)/2.0;
- if(min_vel_theta<=this->ack_steer_speed_min_)
- {
- min_vel_theta=this->ack_steer_speed_min_;
- T4=sim_time_+(2.0*min_vel_theta-this->steering_speed_)/this->ack_steer_acc_max_;
- min_steer=this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)-min_vel_theta*min_vel_theta/this->ack_steer_acc_max_+min_vel_theta*T4+steer_angle;
- if(min_steer<this->ack_steer_angle_min_)
- {
- min_steer=this->ack_steer_angle_min_;
- T4=(this->ack_steer_angle_min_-steer_angle+min_vel_theta*min_vel_theta/this->ack_steer_acc_max_-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_))/min_vel_theta;
- if(T4<0)
- {
- T4=0;
- min_vel_theta=-sqrt(this->steering_speed_*this->steering_speed_/2.0-this->ack_steer_acc_max_*(this->ack_steer_angle_min_-steer_angle));
- }
- }
- }
- else
- {
- min_steer=this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)-min_vel_theta*min_vel_theta/this->ack_steer_acc_max_+steer_angle;
- T4=0;
- if(min_steer<this->ack_steer_angle_min_)
- {
- min_steer=this->ack_steer_angle_min_;
- min_vel_theta=-sqrt(this->steering_speed_*this->steering_speed_/2.0-this->ack_steer_acc_max_*(this->ack_steer_angle_min_-steer_angle));
- }
- }
- }
- else
- {
- max_vel_theta=(this->steering_speed_+this->ack_steer_acc_max_*sim_time_)/2.0;
- if(max_vel_theta>=this->ack_steer_speed_max_)
- {
- max_vel_theta=this->ack_steer_speed_max_;
- T4=sim_time_-(2.0*max_vel_theta-this->steering_speed_)/this->ack_steer_acc_max_;
- max_steer=-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+max_vel_theta*max_vel_theta/this->ack_steer_acc_max_+max_vel_theta*T4+steer_angle;
- if(max_steer>this->ack_steer_angle_max_)
- {
- max_steer=this->ack_steer_angle_max_;
- T4=(this->ack_steer_angle_max_-steer_angle-max_vel_theta*max_vel_theta/this->ack_steer_acc_max_+this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_))/max_vel_theta;
- if(T4<0)
- {
- T4=0;
- max_vel_theta=sqrt(this->steering_speed_*this->steering_speed_/2.0+this->ack_steer_acc_max_*(this->ack_steer_angle_max_-steer_angle));
- }
- }
- }
- else
- {
- max_steer=-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+max_vel_theta*max_vel_theta/this->ack_steer_acc_max_+steer_angle;
- T4=0;
- if(max_steer>this->ack_steer_angle_max_)
- {
- max_steer=this->ack_steer_angle_max_;
- max_vel_theta=sqrt(this->steering_speed_*this->steering_speed_/2.0+this->ack_steer_acc_max_*(this->ack_steer_angle_max_-steer_angle));
- }
- }
- min_vel_theta=(this->steering_speed_-this->ack_steer_acc_max_*sim_time_)/2.0;
- if(min_vel_theta<=this->ack_steer_speed_min_)
- {
- min_vel_theta=this->ack_steer_speed_min_;
- T4=sim_time_+(2.0*min_vel_theta-this->steering_speed_)/this->ack_steer_acc_max_;
- min_steer=-min_vel_theta*min_vel_theta/this->ack_steer_acc_max_+this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+min_vel_theta*T4+steer_angle;
- if(min_steer<this->ack_steer_angle_min_)
- {
- min_steer=this->ack_steer_angle_min_;
- T4=(this->ack_steer_angle_min_-steer_angle+min_vel_theta*min_vel_theta/this->ack_steer_acc_max_-this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_))/min_vel_theta;
- if(T4<0)
- {
- T4=0;
- min_vel_theta=-sqrt(this->steering_speed_*this->steering_speed_/2.0-this->ack_steer_acc_max_*(this->ack_steer_angle_min_-steer_angle));
- }
- }
- }
- else
- {
- min_steer=-min_vel_theta*min_vel_theta/this->ack_steer_acc_max_+this->steering_speed_*this->steering_speed_/(2.0*this->ack_steer_acc_max_)+steer_angle;
- T4=0;
- if(min_steer<this->ack_steer_angle_min_)
- {
- min_steer=this->ack_steer_angle_min_;
- min_vel_theta=-sqrt(this->steering_speed_*this->steering_speed_/2.0-this->ack_steer_acc_max_*(this->ack_steer_angle_min_-steer_angle));
- }
- }
- }
-
- dist=sqrt((map_.goal_x_-x)*(map_.goal_x_-x)+(map_.goal_y_-y)*(map_.goal_y_-y));
- double d=0.0,T5=0.0,a,b,c;
-
- max_vel_x=(v_t+this->ack_acc_max_*sim_time_)/2.0;
- if(max_vel_x>this->ack_vel_max_)
- max_vel_x=this->ack_vel_max_;
- if(v_t>0)
- {
- T5=sim_time_-(2.0*max_vel_x-v_t)/this->ack_acc_max_;
- d=max_vel_x*max_vel_x/this->ack_acc_max_-v_t*v_t/(2.0*this->ack_acc_max_)+max_vel_x*T5;
- if(d>dist)
- {
- d=dist;
- a=1;
- b=-v_t-sim_time_*this->ack_acc_max_;
- c=v_t*v_t/2.0+dist*this->ack_acc_max_;
- max_vel_x=(-b-sqrt(b*b-4*a*c))/(2.0*a);
- }
- }
- else
- {
- T5=sim_time_-(2.0*max_vel_x-v_t)/this->ack_acc_max_;
- d=-v_t*v_t/(2.0*this->ack_acc_max_)+max_vel_x*max_vel_x/this->ack_acc_max_+max_vel_x*T5;
- if(d>dist)
- {
- d=dist;
- a=1;
- b=-v_t-sim_time_*this->ack_acc_max_;
- c=v_t*v_t/2.0+dist*this->ack_acc_max_;
- max_vel_x=(-b-sqrt(b*b-4*a*c))/(2.0*a);
- }
- }
- min_vel_x=-max_vel_x;
- ROS_INFO("TrajectoryPlanner::createTrajectories: Goal_dist: %f, Dist: %f, Max_vel: %f", dist, d, max_vel_x);
- //we want to sample the velocity space regularly
- dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
- dvtheta = (max_steer - min_steer) / (vtheta_samples_ - 1);
-
- double vx_samp = min_vel_x;
- double vtheta_samp = min_steer;
- double vy_samp = 0.0;
-
- //keep track of the best trajectory seen so far
- Trajectory* best_traj = &traj_one;
- best_traj->cost_ = -1.0;
-
- Trajectory* comp_traj = &traj_two;
- comp_traj->cost_ = -1.0;
-
- Trajectory* swap = NULL;
-
- //any cell with a cost greater than the size of the map is impossible
- double impossible_cost = map_.map_.size();
-
- ROS_INFO("TrajectoryPlanner::createTrajectories: CurrentSteerAngle: %f, currentSteerSpeed: %f, CurrentSpeed: %f", vy, vtheta, vx);
- //loop through all x velocities
- for(int i = 0; i < vx_samples_; ++i)
- {
- vtheta_samp = 0;
- /* compute */
- //first sample the straight trajectory
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
- acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
-
- //if the new trajectory is better... let's take it
- if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
- swap = best_traj;
- best_traj = comp_traj;
- comp_traj = swap;
- ROS_DEBUG("TrajectoryPlanner::createTrajectories: vt: %f, SteerAngle: 0, Cost: %f", i*dvx+min_vel_x, comp_traj->cost_);
- }
-
- vtheta_samp = min_steer;
- //next sample all theta trajectories
- for(int j = 0; j < vtheta_samples_ ; ++j){
- generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
- acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
- //if the new trajectory is better... let's take it
- if(comp_traj->cost_ >= 0 && (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0)){
- swap = best_traj;
- best_traj = comp_traj;
- comp_traj = swap;
- ROS_DEBUG("TrajectoryPlanner::createTrajectories: vt: %f, SteerAngle: %f, Cost: %f", i*dvx + min_vel_x, j*dvtheta + min_steer, comp_traj->cost_);
- }
- vtheta_samp += dvtheta;
- }
- vx_samp += dvx;
- }
-
- //next we want to generate trajectories for rotating in place
- vtheta_samp = min_vel_theta;
- vx_samp = 0.0;
- vy_samp = 0.0;
-
- //do we have a legal trajectory
- if(best_traj->cost_ >= 0)
- {
- /* if velocity command is small and the distance to goal is bigger than the threshold -> replan */
- dist=sqrt((map_.goal_x_-x)*(map_.goal_x_-x)+(map_.goal_y_-y)*(map_.goal_y_-y));
- if(dist>xy_goal_tol_ && fabs(best_traj->xv_)<0.02)
- best_traj->cost_=-1;
- return *best_traj;
- }
-
- //if the trajectory failed because the footprint hits something, we're still going to back up
- if(best_traj->cost_ == -1.0)
- {
- best_traj->resetPoints();
- best_traj->xv_ = 0.0;
- best_traj->yv_ = 0.0;
- best_traj->thetav_ = 0.0;
- best_traj->cost_ = -3.0;// no solution, keep the previous command
- }
-
- return *best_traj;
- }
-
- //given the current state of the robot, find a good trajectory
- Trajectory TrajectoryPlanner::findBestPath(tf::Stamped<tf::Pose> global_pose, tf::Stamped<tf::Pose> global_vel,
- tf::Stamped<tf::Pose>& drive_velocities,geometry_msgs::Twist &ackermann_state)
- {
- static Trajectory last_best_traj;
-
- double yaw = tf::getYaw(global_pose.getRotation());
- //double vel_yaw = tf::getYaw(global_vel.getRotation());
-
- double x = global_pose.getOrigin().getX();
- double y = global_pose.getOrigin().getY();
- double theta = yaw;
-
- double vx = ackermann_state.linear.z;
- double vy = ackermann_state.angular.x;
- double vtheta = ackermann_state.angular.y;
-
- //reset the map for new operations
- map_.resetPathDist();
-
- //temporarily remove obstacles that are within the footprint of the robot
- vector<iri_ackermann_local_planner::Position2DInt> footprint_list = getFootprintCells(x, y, theta, true);
-
- //mark cells within the initial footprint of the robot
- for(unsigned int i = 0; i < footprint_list.size(); ++i){
- map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
- }
-
- //make sure that we update our path based on the global plan and compute costs
- map_.setPathCells(costmap_, global_plan_);
- ROS_DEBUG("Path/Goal distance computed");
-
-
- //rollout trajectories and find the minimum cost one
- Trajectory best = createTrajectories(x, y, theta,
- vx, vy, vtheta,
- ack_acc_max_, ack_vel_max_, ack_vel_min_);
- ROS_DEBUG("Trajectories created");
-
- if(best.cost_ < 0){
- if(best.cost_==-3)
- {
- best.xv_=last_best_traj.xv_;
- best.yv_=last_best_traj.yv_;
- best.thetav_=last_best_traj.thetav_;
- best.cost_=last_best_traj.cost_;
- tf::Vector3 start(best.xv_, best.yv_, 0);
- drive_velocities.setOrigin(start);
- tf::Matrix3x3 matrix;
- matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
- drive_velocities.setBasis(matrix);
- }
- else
- drive_velocities.setIdentity();
- }
- else{
- tf::Vector3 start(best.xv_, best.yv_, 0);
- drive_velocities.setOrigin(start);
- tf::Matrix3x3 matrix;
- matrix.setRotation(tf::createQuaternionFromYaw(best.thetav_));
- drive_velocities.setBasis(matrix);
- last_best_traj.xv_=best.xv_;
- last_best_traj.yv_=best.yv_;
- last_best_traj.thetav_=best.thetav_;
- last_best_traj.cost_=best.cost_;
- }
-
- return best;
- }
-
- //we need to take the footprint of the robot into account when we calculate cost to obstacles
- double TrajectoryPlanner::footprintCost(double x_i, double y_i, double theta_i){
- //build the oriented footprint
- double cos_th = cos(theta_i);
- double sin_th = sin(theta_i);
- vector<geometry_msgs::Point> oriented_footprint;
- for(unsigned int i = 0; i < footprint_spec_.size(); ++i){
- geometry_msgs::Point new_pt;
- new_pt.x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
- new_pt.y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
- oriented_footprint.push_back(new_pt);
- }
-
- geometry_msgs::Point robot_position;
- robot_position.x = x_i;
- robot_position.y = y_i;
-
- //check if the footprint is legal
- double footprint_cost = world_model_.footprintCost(robot_position, oriented_footprint, costmap_.getInscribedRadius(), costmap_.getCircumscribedRadius());
-
- return footprint_cost;
- }
-
- void TrajectoryPlanner::getLineCells(int x0, int x1, int y0, int y1, vector<iri_ackermann_local_planner::Position2DInt>& pts){
- //Bresenham Ray-Tracing
- int deltax = abs(x1 - x0); // The difference between the x's
- int deltay = abs(y1 - y0); // The difference between the y's
- int x = x0; // Start x off at the first pixel
- int y = y0; // Start y off at the first pixel
-
- int xinc1, xinc2, yinc1, yinc2;
- int den, num, numadd, numpixels;
-
- iri_ackermann_local_planner::Position2DInt pt;
-
- if (x1 >= x0) // The x-values are increasing
- {
- xinc1 = 1;
- xinc2 = 1;
- }
- else // The x-values are decreasing
- {
- xinc1 = -1;
- xinc2 = -1;
- }
-
- if (y1 >= y0) // The y-values are increasing
- {
- yinc1 = 1;
- yinc2 = 1;
- }
- else // The y-values are decreasing
- {
- yinc1 = -1;
- yinc2 = -1;
- }
-
- if (deltax >= deltay) // There is at least one x-value for every y-value
- {
- xinc1 = 0; // Don't change the x when numerator >= denominator
- yinc2 = 0; // Don't change the y for every iteration
- den = deltax;
- num = deltax / 2;
- numadd = deltay;
- numpixels = deltax; // There are more x-values than y-values
- }
- else // There is at least one y-value for every x-value
- {
- xinc2 = 0; // Don't change the x for every iteration
- yinc1 = 0; // Don't change the y when numerator >= denominator
- den = deltay;
- num = deltay / 2;
- numadd = deltax;
- numpixels = deltay; // There are more y-values than x-values
- }
-
- for (int curpixel = 0; curpixel <= numpixels; curpixel++)
- {
- pt.x = x; //Draw the current pixel
- pt.y = y;
- pts.push_back(pt);
-
- num += numadd; // Increase the numerator by the top of the fraction
- if (num >= den) // Check if numerator >= denominator
- {
- num -= den; // Calculate the new numerator value
- x += xinc1; // Change the x as appropriate
- y += yinc1; // Change the y as appropriate
- }
- x += xinc2; // Change the x as appropriate
- y += yinc2; // Change the y as appropriate
- }
- }
-
- //get the cellsof a footprint at a given position
- vector<iri_ackermann_local_planner::Position2DInt> TrajectoryPlanner::getFootprintCells(double x_i, double y_i, double theta_i, bool fill){
- vector<iri_ackermann_local_planner::Position2DInt> footprint_cells;
-
- //if we have no footprint... do nothing
- if(footprint_spec_.size() <= 1){
- unsigned int mx, my;
- if(costmap_.worldToMap(x_i, y_i, mx, my)){
- Position2DInt center;
- center.x = mx;
- center.y = my;
- footprint_cells.push_back(center);
- }
- return footprint_cells;
- }
-
- //pre-compute cos and sin values
- double cos_th = cos(theta_i);
- double sin_th = sin(theta_i);
- double new_x, new_y;
- unsigned int x0, y0, x1, y1;
- unsigned int last_index = footprint_spec_.size() - 1;
-
- for(unsigned int i = 0; i < last_index; ++i){
- //find the cell coordinates of the first segment point
- new_x = x_i + (footprint_spec_[i].x * cos_th - footprint_spec_[i].y * sin_th);
- new_y = y_i + (footprint_spec_[i].x * sin_th + footprint_spec_[i].y * cos_th);
- if(!costmap_.worldToMap(new_x, new_y, x0, y0))
- return footprint_cells;
-
- //find the cell coordinates of the second segment point
- new_x = x_i + (footprint_spec_[i + 1].x * cos_th - footprint_spec_[i + 1].y * sin_th);
- new_y = y_i + (footprint_spec_[i + 1].x * sin_th + footprint_spec_[i + 1].y * cos_th);
- if(!costmap_.worldToMap(new_x, new_y, x1, y1))
- return footprint_cells;
-
- getLineCells(x0, x1, y0, y1, footprint_cells);
- }
-
- //we need to close the loop, so we also have to raytrace from the last pt to first pt
- new_x = x_i + (footprint_spec_[last_index].x * cos_th - footprint_spec_[last_index].y * sin_th);
- new_y = y_i + (footprint_spec_[last_index].x * sin_th + footprint_spec_[last_index].y * cos_th);
- if(!costmap_.worldToMap(new_x, new_y, x0, y0))
- return footprint_cells;
-
- new_x = x_i + (footprint_spec_[0].x * cos_th - footprint_spec_[0].y * sin_th);
- new_y = y_i + (footprint_spec_[0].x * sin_th + footprint_spec_[0].y * cos_th);
- if(!costmap_.worldToMap(new_x, new_y, x1, y1))
- return footprint_cells;
-
- getLineCells(x0, x1, y0, y1, footprint_cells);
-
- if(fill)
- getFillCells(footprint_cells);
-
- return footprint_cells;
- }
-
- void TrajectoryPlanner::getFillCells(vector<iri_ackermann_local_planner::Position2DInt>& footprint){
- //quick bubble sort to sort pts by x
- iri_ackermann_local_planner::Position2DInt swap, pt;
- unsigned int i = 0;
- while(i < footprint.size() - 1){
- if(footprint[i].x > footprint[i + 1].x){
- swap = footprint[i];
- footprint[i] = footprint[i + 1];
- footprint[i + 1] = swap;
- if(i > 0)
- --i;
- }
- else
- ++i;
- }
-
- i = 0;
- iri_ackermann_local_planner::Position2DInt min_pt;
- iri_ackermann_local_planner::Position2DInt max_pt;
- unsigned int min_x = footprint[0].x;
- unsigned int max_x = footprint[footprint.size() -1].x;
- //walk through each column and mark cells inside the footprint
- for(unsigned int x = min_x; x <= max_x; ++x){
- if(i >= footprint.size() - 1)
- break;
-
- if(footprint[i].y < footprint[i + 1].y){
- min_pt = footprint[i];
- max_pt = footprint[i + 1];
- }
- else{
- min_pt = footprint[i + 1];
- max_pt = footprint[i];
- }
-
- i += 2;
- while(i < footprint.size() && footprint[i].x == x){
- if(footprint[i].y < min_pt.y)
- min_pt = footprint[i];
- else if(footprint[i].y > max_pt.y)
- max_pt = footprint[i];
- ++i;
- }
-
- //loop though cells in the column
- for(unsigned int y = min_pt.y; y < max_pt.y; ++y){
- pt.x = x;
- pt.y = y;
- footprint.push_back(pt);
- }
- }
- }
-
- void TrajectoryPlanner::getLocalGoal(double& x, double& y){
- x = map_.goal_x_;
- y = map_.goal_y_;
- }
-
-};
diff --git a/src/trajectory_planner_ros.cpp b/src/trajectory_planner_ros.cpp
deleted file mode 100755
index a576bc020304068d43613f4b7d83987e22b5f3f1..0000000000000000000000000000000000000000
--- a/src/trajectory_planner_ros.cpp
+++ /dev/null
@@ -1,490 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-
-#include <trajectory_planner_ros.h>
-#include <ros/console.h>
-#include <sys/time.h>
-#include <pluginlib/class_list_macros.h>
-#include <boost/tokenizer.hpp>
-
-#include "geometry_msgs/PolygonStamped.h"
-#include "nav_msgs/Path.h"
-#include "goal_functions.h"
-
-using namespace std;
-using namespace costmap_2d;
-
-//register this planner as a BaseLocalPlanner plugin
-PLUGINLIB_DECLARE_CLASS(iri_ackermann_local_planner, TrajectoryPlannerROS, iri_ackermann_local_planner::TrajectoryPlannerROS, nav_core::BaseLocalPlanner)
-
-namespace iri_ackermann_local_planner {
-
- void TrajectoryPlannerROS::reconfigureCB(AckermannLocalPlannerConfig &config, uint32_t level) {
- if(setup_ && config.restore_defaults) {
- config = default_config_;
- //Avoid looping
- config.restore_defaults = false;
- }
- if(!setup_) {
- default_config_ = config;
- setup_ = true;
- }
- else if(setup_) {
- tc_->reconfigure(config);
- }
- }
-
- TrajectoryPlannerROS::TrajectoryPlannerROS() : world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), initialized_(false), setup_(false)
- {
- }
-
- TrajectoryPlannerROS::TrajectoryPlannerROS(std::string name, tf::TransformListener* tf, Costmap2DROS* costmap_ros)
- : world_model_(NULL), tc_(NULL), costmap_ros_(NULL), tf_(NULL), initialized_(false), setup_(false) {
- //initialize the planner
- initialize(name, tf, costmap_ros);
- }
-
- void TrajectoryPlannerROS::initialize(std::string name, tf::TransformListener* tf, Costmap2DROS* costmap_ros){
- if(!initialized_)
- {
- tf_ = tf;
- costmap_ros_ = costmap_ros;
- rot_stopped_velocity_ = 1e-2;
- trans_stopped_velocity_ = 1e-2;
- double sim_time, sim_granularity, angular_sim_granularity;
- int vx_samples, vtheta_samples;
- double pdist_scale, gdist_scale, occdist_scale, hdiff_scale;
- double ack_acc_max,ack_vel_max,ack_vel_min;
- double ack_steer_acc_max,ack_steer_vel_max,ack_steer_vel_min,ack_steer_angle_max,ack_steer_angle_min;
- double axis_distance;
- bool simple_attractor;
- int heading_points;
- string world_model_type;
- rotating_to_goal_ = false;
-
- //initialize the copy of the costmap the controller will use
- costmap_ros_->getCostmapCopy(costmap_);
-
- ros::NodeHandle private_nh("~/" + name);
-
- g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
- l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
-
- global_frame_ = costmap_ros_->getGlobalFrameID();
- robot_base_frame_ = costmap_ros_->getBaseFrameID();
- private_nh.param("prune_plan", prune_plan_, true);
-
- private_nh.param("yaw_goal_tolerance", yaw_goal_tolerance_, 0.05);
- private_nh.param("xy_goal_tolerance", xy_goal_tolerance_, 0.10);
-
- //we'll get the parameters for the robot radius from the costmap we're associated with
- inflation_radius_ = costmap_ros_->getInflationRadius();
-
- private_nh.param("ack_acc_max", ack_acc_max, 1.0);
- private_nh.param("ack_vel_max", ack_vel_max, 0.6);
- private_nh.param("ack_vel_min", ack_vel_min, -0.6);
- private_nh.param("ack_steer_acc_max", ack_steer_acc_max, 1.0);
- private_nh.param("ack_steer_speed_max", ack_steer_vel_max, 0.5);
- private_nh.param("ack_steer_speed_min", ack_steer_vel_min, -0.5);
- private_nh.param("ack_steer_angle_max", ack_steer_angle_max, 0.35);
- private_nh.param("ack_steer_angle_min", ack_steer_angle_min, -0.35);
- private_nh.param("ack_axis_distance", axis_distance, 1.65);
-
- private_nh.param("latch_xy_goal_tolerance", latch_xy_goal_tolerance_, false);
-
- //Assuming this planner is being run within the navigation stack, we can
- //just do an upward search for the frequency at which its being run. This
- //also allows the frequency to be overwritten locally.
- std::string controller_frequency_param_name;
-
- private_nh.param("sim_time", sim_time, 10.0);
- private_nh.param("sim_granularity", sim_granularity, 0.025);
- private_nh.param("angular_sim_granularity", angular_sim_granularity, sim_granularity);
- private_nh.param("vx_samples", vx_samples, 10);//3
- private_nh.param("vtheta_samples", vtheta_samples, 20);
-
- private_nh.param("pdist_scale", pdist_scale, 0.6);
- private_nh.param("gdist_scale", gdist_scale, 0.8);
- private_nh.param("hdiff_scale", hdiff_scale, 1.0);
- private_nh.param("occdist_scale", occdist_scale, 0.01);
- private_nh.param("heading_points", heading_points, 8);
-
- private_nh.param("world_model", world_model_type, string("costmap"));
-
- simple_attractor = false;
-
- //parameters for using the freespace controller
- double min_pt_separation, max_obstacle_height, grid_resolution;
- private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
- private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
- private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
- private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
-
- ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
- world_model_ = new CostmapModel(costmap_);
-
- tc_ = new TrajectoryPlanner(*world_model_, costmap_, costmap_ros_->getRobotFootprint(),
- ack_acc_max, ack_vel_max, ack_vel_min, ack_steer_acc_max,ack_steer_vel_max,ack_steer_vel_min,ack_steer_angle_max,ack_steer_angle_min,axis_distance,
- sim_time, sim_granularity, vx_samples, vtheta_samples, pdist_scale,gdist_scale, occdist_scale, hdiff_scale,simple_attractor,angular_sim_granularity,
- heading_points,xy_goal_tolerance_);
-
- map_viz_.initialize(name, &costmap_, boost::bind(&TrajectoryPlanner::getCellCosts, tc_, _1, _2, _3, _4, _5, _6));
- initialized_ = true;
-
- dsrv_ = new dynamic_reconfigure::Server<AckermannLocalPlannerConfig>(private_nh);
- dynamic_reconfigure::Server<AckermannLocalPlannerConfig>::CallbackType cb = boost::bind(&TrajectoryPlannerROS::reconfigureCB, this, _1, _2);
- dsrv_->setCallback(cb);
-
- ros::NodeHandle global_node;
- odom_sub_ = global_node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&TrajectoryPlannerROS::odomCallback, this, _1));
- }
- else
- ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
- }
-
- TrajectoryPlannerROS::~TrajectoryPlannerROS(){
- //make sure to clean things up
- delete dsrv_;
-
- if(tc_ != NULL)
- delete tc_;
-
- if(world_model_ != NULL)
- delete world_model_;
- }
-
- void TrajectoryPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
- //we assume that the odometry is published in the frame of the base
- boost::recursive_mutex::scoped_lock lock(odom_lock_);
- base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
- base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
- base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
- ackermann_state_.linear.z=msg->twist.twist.linear.z;
- ackermann_state_.angular.x=msg->twist.twist.angular.x;
- ackermann_state_.angular.y=msg->twist.twist.angular.y;
- ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
- base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
- }
-
- bool TrajectoryPlannerROS::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
- {
- if(!initialized_)
- {
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
- return false;
- }
-
- ////divide plan by manuveurs
- subPathList.clear();
- subPath.clear();
- subPathIndex = 0;
- double pathLength = 0;
- for(unsigned int i = 0; i < orig_global_plan.size(); ++i)
- {
- if(i>1 && i<orig_global_plan.size())
- {
- double x0 = orig_global_plan[i ].pose.position.x;
- double x1 = orig_global_plan[i-1].pose.position.x;
- double x2 = orig_global_plan[i-2].pose.position.x;
- double y0 = orig_global_plan[i ].pose.position.y;
- double y1 = orig_global_plan[i-1].pose.position.y;
- double y2 = orig_global_plan[i-2].pose.position.y;
- double angle=std::acos(((x0-x1)*(x1-x2)+(y0-y1)*(y1-y2))/(std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2))*std::sqrt(std::pow(x1-x2,2)+std::pow(y1-y2,2))));
- pathLength+= std::sqrt(std::pow(x0-x1,2)+std::pow(y0-y1,2));
- if(fabs(angle)>1.57) //if changes of direction detected
- {
- if(pathLength>1.0)
- {
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath split at i=%d, angle=%f, length=%f", i, angle, pathLength);
- subPathList.push_back(subPath);
- }
- else //ignored subpaths shorter than 1.0m
- {
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath split at i=%d, angle=%f, Ignored by length=%f", i, angle, pathLength);
- }
- subPath.clear();
- pathLength=0.0;
- }
- }
- subPath.push_back(orig_global_plan[i]);
- }
- subPathList.push_back(subPath);
- ROS_INFO("TrajectoryPlannerROS::setPlan: subPath last length=%f", pathLength);
- ROS_INFO("TrajectoryPlannerROS::setPlan: Global plan (%lu points) split in %lu paths", orig_global_plan.size(), subPathList.size());
- global_plan_.clear();
- global_plan_ = subPathList[subPathIndex];
- ////
-
- //reset the global plan
- ////global_plan_.clear();
- ////global_plan_ = orig_global_plan;
-
- //when we get a new plan, we also want to clear any latch we may have on goal tolerances
- xy_tolerance_latch_ = false;
-
- return true;
- }
-
- bool TrajectoryPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
- if(!initialized_){
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
- return false;
- }
- ROS_INFO("TrajectoryPlannerROS::computeVelocityCommands!");
-
- std::vector<geometry_msgs::PoseStamped> local_plan;
- tf::Stamped<tf::Pose> global_pose;
- if(!costmap_ros_->getRobotPose(global_pose))
- return false;
-
- std::vector<geometry_msgs::PoseStamped> transformed_plan;
- //get the global plan in our frame
- if(!transformGlobalPlan(*tf_, global_plan_, *costmap_ros_, global_frame_, transformed_plan)){
- ROS_WARN("Could not transform the global plan to the frame of the controller");
- return false;
- }
-
- //now we'll prune the plan based on the position of the robot
- if(prune_plan_)
- prunePlan(global_pose, transformed_plan, global_plan_);
-
-
- //we also want to clear the robot footprint from the costmap we're using
- costmap_ros_->clearRobotFootprint();
-
- //make sure to update the costmap we'll use for this cycle
- costmap_ros_->getCostmapCopy(costmap_);
-
- // Set current velocities from odometry
- geometry_msgs::Twist global_vel;
-
- odom_lock_.lock();
- global_vel.linear.x = base_odom_.twist.twist.linear.x;
- global_vel.linear.y = base_odom_.twist.twist.linear.y;
- global_vel.angular.z = base_odom_.twist.twist.angular.z;
- odom_lock_.unlock();
-
- tf::Stamped<tf::Pose> drive_cmds;
- drive_cmds.frame_id_ = robot_base_frame_;
-
- tf::Stamped<tf::Pose> robot_vel;
- robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
- robot_vel.frame_id_ = robot_base_frame_;
- robot_vel.stamp_ = ros::Time();
-
- //if the global plan passed in is empty... we won't do anything
- if(transformed_plan.empty())
- return false;
-
- tf::Stamped<tf::Pose> goal_point;
- tf::poseStampedMsgToTF(transformed_plan.back(), goal_point);
- //we assume the global goal is the last point in the global plan
- double goal_x = goal_point.getOrigin().getX();
- double goal_y = goal_point.getOrigin().getY();
-
- double yaw = tf::getYaw(goal_point.getRotation());
-
- double goal_th = yaw;
-
- //check to see if we've reached the goal position
- if(goalPositionReached(global_pose, goal_x, goal_y, xy_goal_tolerance_) || xy_tolerance_latch_){
-
- ////check if there are manuveurs remaining
- if(subPathIndex < subPathList.size()-1)
- {
- subPathIndex++;
- global_plan_.clear();
- global_plan_ = subPathList[subPathIndex];
- return true;
- }
- ////
-
- //if the user wants to latch goal tolerance, if we ever reach the goal location, we'll
- //just rotate in place
- if(latch_xy_goal_tolerance_)
- xy_tolerance_latch_ = true;
-
- //check to see if the goal orientation has been reached
- if(goalOrientationReached(global_pose, goal_th, yaw_goal_tolerance_)){
- //set the velocity command to zero
- cmd_vel.linear.x = 0.0;
- cmd_vel.linear.y = 0.0;
- cmd_vel.angular.z = 0.0;
- rotating_to_goal_ = false;
- xy_tolerance_latch_ = false;
- }
-
- //publish an empty plan because we've reached our goal position
- publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
-
- //we don't actually want to run the controller when we're just rotating to goal
- return true;
- }
-
- tc_->updatePlan(transformed_plan);
-
- //compute what trajectory to drive along
- Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds,ackermann_state_);
-
- ROS_INFO("Speed command x,y,yaw: %f,%f,%f",drive_cmds.getOrigin().getX(),drive_cmds.getOrigin().getY(),tf::getYaw(drive_cmds.getRotation()));
-
- map_viz_.publishCostCloud();
-
- //pass along drive commands
- cmd_vel.linear.x = drive_cmds.getOrigin().getX();
- cmd_vel.linear.y = drive_cmds.getOrigin().getY();
- yaw = tf::getYaw(drive_cmds.getRotation());
-
- cmd_vel.angular.z = yaw;
-
- //if we cannot move... tell someone
- if(path.cost_ < 0){
- local_plan.clear();
- publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
- return false;
- }
-
- // Fill out the local plan
- for(unsigned int i = 0; i < path.getPointsSize(); ++i){
- double p_x, p_y, p_th;
- path.getPoint(i, p_x, p_y, p_th);
-
- tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::createQuaternionFromYaw(p_th), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_);
- geometry_msgs::PoseStamped pose;
- tf::poseStampedTFToMsg(p, pose);
- local_plan.push_back(pose);
- }
-
- //publish information to the visualizer
- publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
- publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
- return true;
- }
-
- bool TrajectoryPlannerROS::checkTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
- tf::Stamped<tf::Pose> global_pose;
- if(costmap_ros_->getRobotPose(global_pose)){
- if(update_map){
- //we also want to clear the robot footprint from the costmap we're using
- costmap_ros_->clearRobotFootprint();
-
- //make sure to update the costmap we'll use for this cycle
- costmap_ros_->getCostmapCopy(costmap_);
-
- //we need to give the planne some sort of global plan, since we're only checking for legality
- //we'll just give the robots current position
- std::vector<geometry_msgs::PoseStamped> plan;
- geometry_msgs::PoseStamped pose_msg;
- tf::poseStampedTFToMsg(global_pose, pose_msg);
- plan.push_back(pose_msg);
- tc_->updatePlan(plan, true);
- }
-
- //copy over the odometry information
- nav_msgs::Odometry base_odom;
- {
- boost::recursive_mutex::scoped_lock lock(odom_lock_);
- base_odom = base_odom_;
- }
-
- return tc_->checkTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
- base_odom.twist.twist.linear.x,
- base_odom.twist.twist.linear.y,
- base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
-
- }
- ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
- return false;
- }
-
-
- double TrajectoryPlannerROS::scoreTrajectory(double vx_samp, double vy_samp, double vtheta_samp, bool update_map){
- // Copy of checkTrajectory that returns a score instead of True / False
- tf::Stamped<tf::Pose> global_pose;
- if(costmap_ros_->getRobotPose(global_pose)){
- if(update_map){
- //we also want to clear the robot footprint from the costmap we're using
- costmap_ros_->clearRobotFootprint();
-
- //make sure to update the costmap we'll use for this cycle
- costmap_ros_->getCostmapCopy(costmap_);
-
- //we need to give the planne some sort of global plan, since we're only checking for legality
- //we'll just give the robots current position
- std::vector<geometry_msgs::PoseStamped> plan;
- geometry_msgs::PoseStamped pose_msg;
- tf::poseStampedTFToMsg(global_pose, pose_msg);
- plan.push_back(pose_msg);
- tc_->updatePlan(plan, true);
- }
-
- //copy over the odometry information
- nav_msgs::Odometry base_odom;
- {
- boost::recursive_mutex::scoped_lock lock(odom_lock_);
- base_odom = base_odom_;
- }
-
- return tc_->scoreTrajectory(global_pose.getOrigin().x(), global_pose.getOrigin().y(), tf::getYaw(global_pose.getRotation()),
- base_odom.twist.twist.linear.x,
- base_odom.twist.twist.linear.y,
- base_odom.twist.twist.angular.z, vx_samp, vy_samp, vtheta_samp);
-
- }
- ROS_WARN("Failed to get the pose of the robot. No trajectories will pass as legal in this case.");
- return -1.0;
- }
-
- bool TrajectoryPlannerROS::isGoalReached(){
- if(!initialized_){
- ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
- return false;
- }
-
- //copy over the odometry information
- nav_msgs::Odometry base_odom;
- {
- boost::recursive_mutex::scoped_lock lock(odom_lock_);
- base_odom = base_odom_;
- }
-
- return iri_ackermann_local_planner::isGoalReached(*tf_, global_plan_, *costmap_ros_, global_frame_, base_odom,
- rot_stopped_velocity_, trans_stopped_velocity_, xy_goal_tolerance_, yaw_goal_tolerance_);
- }
-};
diff --git a/src/voxel_grid_model.cpp b/src/voxel_grid_model.cpp
deleted file mode 100755
index 1b478f18a1cb7462fc6984800cf165be0801abc2..0000000000000000000000000000000000000000
--- a/src/voxel_grid_model.cpp
+++ /dev/null
@@ -1,287 +0,0 @@
-/*********************************************************************
-*
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*
-* Author: Eitan Marder-Eppstein
-*********************************************************************/
-#include <voxel_grid_model.h>
-
-using namespace std;
-using namespace costmap_2d;
-
-namespace iri_ackermann_local_planner {
- VoxelGridModel::VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
- double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range) :
- obstacle_grid_(size_x, size_y, size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution),
- origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z),
- max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range) {}
-
- double VoxelGridModel::footprintCost(const geometry_msgs::Point& position, const vector<geometry_msgs::Point>& footprint,
- double inscribed_radius, double circumscribed_radius){
- if(footprint.size() < 3)
- return -1.0;
-
- //now we really have to lay down the footprint in the costmap grid
- unsigned int x0, x1, y0, y1;
- double line_cost = 0.0;
-
- //we need to rasterize each line in the footprint
- for(unsigned int i = 0; i < footprint.size() - 1; ++i){
- //get the cell coord of the first point
- if(!worldToMap2D(footprint[i].x, footprint[i].y, x0, y0))
- return -1.0;
-
- //get the cell coord of the second point
- if(!worldToMap2D(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
- return -1.0;
-
- line_cost = lineCost(x0, x1, y0, y1);
-
- //if there is an obstacle that hits the line... we know that we can return false right away
- if(line_cost < 0)
- return -1.0;
- }
-
- //we also need to connect the first point in the footprint to the last point
- //get the cell coord of the last point
- if(!worldToMap2D(footprint.back().x, footprint.back().y, x0, y0))
- return -1.0;
-
- //get the cell coord of the first point
- if(!worldToMap2D(footprint.front().x, footprint.front().y, x1, y1))
- return -1.0;
-
- line_cost = lineCost(x0, x1, y0, y1);
-
- if(line_cost < 0)
- return -1.0;
-
- //if all line costs are legal... then we can return that the footprint is legal
- return 0.0;
- }
-
- //calculate the cost of a ray-traced line
- double VoxelGridModel::lineCost(int x0, int x1,
- int y0, int y1){
- //Bresenham Ray-Tracing
- int deltax = abs(x1 - x0); // The difference between the x's
- int deltay = abs(y1 - y0); // The difference between the y's
- int x = x0; // Start x off at the first pixel
- int y = y0; // Start y off at the first pixel
-
- int xinc1, xinc2, yinc1, yinc2;
- int den, num, numadd, numpixels;
-
- double line_cost = 0.0;
- double point_cost = -1.0;
-
- if (x1 >= x0) // The x-values are increasing
- {
- xinc1 = 1;
- xinc2 = 1;
- }
- else // The x-values are decreasing
- {
- xinc1 = -1;
- xinc2 = -1;
- }
-
- if (y1 >= y0) // The y-values are increasing
- {
- yinc1 = 1;
- yinc2 = 1;
- }
- else // The y-values are decreasing
- {
- yinc1 = -1;
- yinc2 = -1;
- }
-
- if (deltax >= deltay) // There is at least one x-value for every y-value
- {
- xinc1 = 0; // Don't change the x when numerator >= denominator
- yinc2 = 0; // Don't change the y for every iteration
- den = deltax;
- num = deltax / 2;
- numadd = deltay;
- numpixels = deltax; // There are more x-values than y-values
- }
- else // There is at least one y-value for every x-value
- {
- xinc2 = 0; // Don't change the x for every iteration
- yinc1 = 0; // Don't change the y when numerator >= denominator
- den = deltay;
- num = deltay / 2;
- numadd = deltax;
- numpixels = deltay; // There are more y-values than x-values
- }
-
- for (int curpixel = 0; curpixel <= numpixels; curpixel++)
- {
- point_cost = pointCost(x, y); //Score the current point
-
- if(point_cost < 0)
- return -1;
-
- if(line_cost < point_cost)
- line_cost = point_cost;
-
- num += numadd; // Increase the numerator by the top of the fraction
- if (num >= den) // Check if numerator >= denominator
- {
- num -= den; // Calculate the new numerator value
- x += xinc1; // Change the x as appropriate
- y += yinc1; // Change the y as appropriate
- }
- x += xinc2; // Change the x as appropriate
- y += yinc2; // Change the y as appropriate
- }
-
- return line_cost;
- }
-
- double VoxelGridModel::pointCost(int x, int y){
- //if the cell is in an obstacle the path is invalid
- if(obstacle_grid_.getVoxelColumn(x, y)){
- return -1;
- }
-
- return 1;
- }
-
- void VoxelGridModel::updateWorld(const vector<geometry_msgs::Point>& footprint,
- const vector<Observation>& observations, const vector<PlanarLaserScan>& laser_scans){
-
- //remove points in the laser scan boundry
- for(unsigned int i = 0; i < laser_scans.size(); ++i)
- removePointsInScanBoundry(laser_scans[i], 10.0);
-
- //iterate through all observations and update the grid
- for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
- const Observation& obs = *it;
- const pcl::PointCloud<pcl::PointXYZ>& cloud = obs.cloud_;
- for(unsigned int i = 0; i < cloud.points.size(); ++i){
- //filter out points that are too high
- if(cloud.points[i].z > max_z_)
- continue;
-
- //compute the squared distance from the hitpoint to the pointcloud's origin
- double sq_dist = (cloud.points[i].x - obs.origin_.x) * (cloud.points[i].x - obs.origin_.x)
- + (cloud.points[i].y - obs.origin_.y) * (cloud.points[i].y - obs.origin_.y)
- + (cloud.points[i].z - obs.origin_.z) * (cloud.points[i].z - obs.origin_.z);
-
- if(sq_dist >= sq_obstacle_range_)
- continue;
-
- //insert the point
- insert(cloud.points[i]);
- }
- }
-
- //remove the points that are in the footprint of the robot
- //removePointsInPolygon(footprint);
- }
-
- void VoxelGridModel::removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range){
- if(laser_scan.cloud.points.size() == 0)
- return;
-
- unsigned int sensor_x, sensor_y, sensor_z;
- double ox = laser_scan.origin.x;
- double oy = laser_scan.origin.y;
- double oz = laser_scan.origin.z;
-
- if(!worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
- return;
-
- for(unsigned int i = 0; i < laser_scan.cloud.points.size(); ++i){
- double wpx = laser_scan.cloud.points[i].x;
- double wpy = laser_scan.cloud.points[i].y;
- double wpz = laser_scan.cloud.points[i].z;
-
- double distance = dist(ox, oy, oz, wpx, wpy, wpz);
- double scaling_fact = raytrace_range / distance;
- scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
- wpx = scaling_fact * (wpx - ox) + ox;
- wpy = scaling_fact * (wpy - oy) + oy;
- wpz = scaling_fact * (wpz - oz) + oz;
-
- //we can only raytrace to a maximum z height
- if(wpz >= max_z_){
- //we know we want the vector's z value to be max_z
- double a = wpx - ox;
- double b = wpy - oy;
- double c = wpz - oz;
- double t = (max_z_ - .01 - oz) / c;
- wpx = ox + a * t;
- wpy = oy + b * t;
- wpz = oz + c * t;
- }
- //and we can only raytrace down to the floor
- else if(wpz < 0.0){
- //we know we want the vector's z value to be 0.0
- double a = wpx - ox;
- double b = wpy - oy;
- double c = wpz - oz;
- double t = (0.0 - oz) / c;
- wpx = ox + a * t;
- wpy = oy + b * t;
- wpz = oz + c * t;
- }
-
- unsigned int point_x, point_y, point_z;
- if(worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
- obstacle_grid_.clearVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);
- }
- }
- }
-
- void VoxelGridModel::getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud){
- for(unsigned int i = 0; i < obstacle_grid_.sizeX(); ++i){
- for(unsigned int j = 0; j < obstacle_grid_.sizeY(); ++j){
- for(unsigned int k = 0; k < obstacle_grid_.sizeZ(); ++k){
- if(obstacle_grid_.getVoxel(i, j, k)){
- double wx, wy, wz;
- mapToWorld3D(i, j, k, wx, wy, wz);
- pcl::PointXYZ pt;
- pt.x = wx;
- pt.y = wy;
- pt.z = wz;
- cloud.points.push_back(pt);
- }
- }
- }
- }
- }
-
-};