Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3967] pkg/trunk
|
From: <ei...@us...> - 2008-09-05 00:02:56
|
Revision: 3967
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3967&view=rev
Author: eitanme
Date: 2008-09-05 00:03:02 +0000 (Fri, 05 Sep 2008)
Log Message:
-----------
Initital commit for PR2 local controller
Added Paths:
-----------
pkg/trunk/trajectory_rollout/
pkg/trunk/trajectory_rollout/CMakeLists.txt
pkg/trunk/trajectory_rollout/Makefile
pkg/trunk/trajectory_rollout/include/
pkg/trunk/trajectory_rollout/include/map_cell.h
pkg/trunk/trajectory_rollout/include/map_grid.h
pkg/trunk/trajectory_rollout/include/trajectory.h
pkg/trunk/trajectory_rollout/include/trajectory_controller.h
pkg/trunk/trajectory_rollout/include/trajectory_inc.h
pkg/trunk/trajectory_rollout/lib/
pkg/trunk/trajectory_rollout/manifest.xml
pkg/trunk/trajectory_rollout/msg/
pkg/trunk/trajectory_rollout/src/
pkg/trunk/trajectory_rollout/src/map_cell.cpp
pkg/trunk/trajectory_rollout/src/map_grid.cpp
pkg/trunk/trajectory_rollout/src/tests.cpp
pkg/trunk/trajectory_rollout/src/trajectory.cpp
pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp
Added: pkg/trunk/trajectory_rollout/CMakeLists.txt
===================================================================
--- pkg/trunk/trajectory_rollout/CMakeLists.txt (rev 0)
+++ pkg/trunk/trajectory_rollout/CMakeLists.txt 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(trajectory_rollout)
+
+rospack_add_library(trajectory_rollout src/map_cell.cpp src/map_grid.cpp src/trajectory_controller.cpp src/trajectory.cpp)
+rospack_add_executable(tests src/tests.cpp src/map_cell.cpp src/map_grid.cpp src/trajectory_controller.cpp src/trajectory.cpp)
Added: pkg/trunk/trajectory_rollout/Makefile
===================================================================
--- pkg/trunk/trajectory_rollout/Makefile (rev 0)
+++ pkg/trunk/trajectory_rollout/Makefile 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/trajectory_rollout/include/map_cell.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/map_cell.h (rev 0)
+++ pkg/trunk/trajectory_rollout/include/map_cell.h 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,63 @@
+/*********************************************************************
+ * 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 MAP_CELL_H_
+#define MAP_CELL_H_
+
+#include "trajectory_inc.h"
+
+//Information contained in each map cell
+class MapCell{
+ public:
+ //default constructor
+ MapCell();
+
+ MapCell(const MapCell& mc);
+ MapCell(const MapCell* mc);
+
+ //cell index in the grid map
+ unsigned ci, cj;
+
+ //distance to planner's path
+ double path_dist;
+
+ //distance to goal
+ double goal_dist;
+
+ //occupancy state (-1 = free, 0 = unknown, 1 = occupied)
+ int occ_state;
+
+ //compares two cells based on path_dist so we can use stl priority queues
+ const bool operator< (const MapCell& mc) const;
+};
+#endif
Added: pkg/trunk/trajectory_rollout/include/map_grid.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/map_grid.h (rev 0)
+++ pkg/trunk/trajectory_rollout/include/map_grid.h 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,68 @@
+/*********************************************************************
+ * 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 MAP_GRID_H_
+#define MAP_GRID_H_
+
+#include <vector>
+#include <iostream>
+#include "assert.h"
+#include "trajectory_inc.h"
+
+#include "map_cell.h"
+
+//A grid of MapCells
+class MapGrid{
+ public:
+ MapGrid(unsigned rows, unsigned cols);
+
+ //cells will be accessed by (row, col)
+ MapCell& operator() (unsigned row, unsigned col);
+ MapCell operator() (unsigned row, unsigned col) const;
+
+ ~MapGrid(){}
+
+ MapGrid(const MapGrid& mg);
+ MapGrid& operator= (const MapGrid& mg);
+
+ unsigned rows_, cols_;
+ std::vector<MapCell> map_;
+
+ //lower left corner of grid in world space
+ double origin_x, origin_y;
+
+ //grid scale in meters/cell
+ double scale;
+};
+
+#endif
Added: pkg/trunk/trajectory_rollout/include/trajectory.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/trajectory.h (rev 0)
+++ pkg/trunk/trajectory_rollout/include/trajectory.h 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,73 @@
+/*********************************************************************
+ * 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_H_
+#define TRAJECTORY_H_
+
+#include <vector>
+
+//stores the information for a point on a trajectory
+class TrajectoryPoint{
+ public:
+ TrajectoryPoint(double t, double x, double y, double theta,
+ double xv, double yv, double thetav)
+ : t_(t), x_(x), y_(y), theta_(theta), xv_(xv), yv_(yv), thetav_(thetav)
+ {}
+
+ TrajectoryPoint()
+ : t_(0), x_(0), y_(0), theta_(0), xv_(0), yv_(0), thetav_(0)
+ {}
+
+ //time
+ double t_;
+
+ //position at time t
+ double x_, y_, theta_;
+
+ //velocity at time t
+ double xv_, yv_, thetav_;
+};
+
+//holds a trajectory generated by an x, y, and theta velocity
+class Trajectory {
+ public:
+ Trajectory(double xv, double yv, double thetav, double steps);
+
+ void addPoint(int step, TrajectoryPoint tp);
+
+ double xv_, yv_, thetav_;
+
+ //storage for points in the trajectory
+ std::vector<TrajectoryPoint> points_;
+};
+#endif
Added: pkg/trunk/trajectory_rollout/include/trajectory_controller.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/trajectory_controller.h (rev 0)
+++ pkg/trunk/trajectory_rollout/include/trajectory_controller.h 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,90 @@
+/*********************************************************************
+ * 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_CONTROLLER_H_
+#define TRAJECTORY_CONTROLLER_H_
+
+#include <vector>
+#include <iostream>
+#include <math.h>
+#include <utility>
+
+#include "map_cell.h"
+#include "map_grid.h"
+#include "trajectory.h"
+
+#define SIM_STEPS 20
+#define SIM_TIME 2 //in seconds
+
+#define MAX(x, y) x > y ? x : y
+#define MIN(x, y) x < y ? x : y
+#define VALID_CELL(map, i, j) ((i >= 0) && (i < (int)map.rows_) && (j >= 0) && (j < (int)map.cols_))
+
+//Based on the plan from the path planner, determine what velocities to send to the robot
+class TrajectoryController {
+ public:
+ //create a controller given a map, path, and acceleration limits of the robot
+ TrajectoryController(MapGrid mg, std::vector<std::pair<int, int> > path, double acc_x, double acc_y, double acc_theta);
+
+ //compute the distance from each cell in the map grid to the planned path
+ void computePathDistance();
+
+ //compute the distance from an individual cell to the planned path
+ void cellPathDistance(MapCell* current, int di, int dj);
+
+ //update neighboring path distance
+ void updateNeighbors(MapCell* current);
+
+ //create a trajectory given the current pose of the robot and selected velocities
+ void generateTrajectory(double x, double y, double theta, double vx, double vy,
+ double vtheta, double num_steps, double sim_time);
+
+ //compute position based on velocity
+ double computeNewPosition(double xi, double v, double dt);
+
+ //compute velocity based on acceleration
+ double computeNewVelocity(double vg, double vi, double amax, double dt);
+
+ //the map passed on from the planner
+ MapGrid map_;
+
+ //the path computed by the planner
+ std::vector<std::pair<int, int> > planned_path_;
+
+ //the acceleration limits of the robot
+ double acc_x_, acc_y_, acc_theta_;
+
+};
+
+
+#endif
Added: pkg/trunk/trajectory_rollout/include/trajectory_inc.h
===================================================================
--- pkg/trunk/trajectory_rollout/include/trajectory_inc.h (rev 0)
+++ pkg/trunk/trajectory_rollout/include/trajectory_inc.h 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,47 @@
+/*********************************************************************
+ * 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
Added: pkg/trunk/trajectory_rollout/manifest.xml
===================================================================
--- pkg/trunk/trajectory_rollout/manifest.xml (rev 0)
+++ pkg/trunk/trajectory_rollout/manifest.xml 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,17 @@
+<package>
+ <description>
+ A controller that drives a mobile base in the plane. Given a path from the path
+ planner, the controller determines the dx, dy, dtheta velocities to send to the
+ robot.
+ </description>
+ <author>Eitan Marder-Eppstein</author>
+ <license>BSD</license>
+ <depend package="stl_utils" />
+ <depend package="std_msgs" />
+ <depend package="xmlparam" />
+ <url>http://pr.willowgarage.com</url>
+ <repository>http://pr.willowgarage.com/repos</repository>
+ <export>
+ <cpp cflags='-I${prefix}/include' />
+ </export>
+</package>
Added: pkg/trunk/trajectory_rollout/src/map_cell.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/map_cell.cpp (rev 0)
+++ pkg/trunk/trajectory_rollout/src/map_cell.cpp 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,53 @@
+/*********************************************************************
+ * 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"
+
+MapCell::MapCell()
+ : ci(0), cj(0), path_dist(DBL_MAX), goal_dist(DBL_MAX), occ_state(0)
+{}
+
+MapCell::MapCell(const MapCell& mc)
+ : ci(mc.ci), cj(mc.cj), path_dist(mc.path_dist), goal_dist(mc.goal_dist),
+ occ_state(mc.occ_state)
+{}
+
+MapCell::MapCell(const MapCell* mc)
+ : ci(mc->ci), cj(mc->cj), path_dist(mc->path_dist), goal_dist(mc->goal_dist),
+ occ_state(mc->occ_state)
+{}
+
+const bool MapCell::operator< (const MapCell& mc) const{
+ return path_dist < mc.path_dist;
+}
Added: pkg/trunk/trajectory_rollout/src/map_grid.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/map_grid.cpp (rev 0)
+++ pkg/trunk/trajectory_rollout/src/map_grid.cpp 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,79 @@
+/*********************************************************************
+ * 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;
+
+MapGrid::MapGrid(unsigned rows, unsigned cols)
+ : rows_(rows), cols_(cols)
+{
+ //don't allow construction of zero size grid
+ assert(rows != 0 && cols != 0);
+
+ map_.resize(rows * cols);
+
+ //make each cell aware of its location in the grid
+ for(unsigned int i = 0; i < rows; ++i){
+ for(unsigned int j = 0; j < cols; ++ j){
+ map_[cols * i + j].ci = i;
+ map_[cols * i + j].cj = j;
+ }
+ }
+}
+
+MapCell& MapGrid::operator() (unsigned row, unsigned col){
+ assert(row < rows_ && col < cols_);
+ //check for legal index
+ return map_[cols_ * row + col];
+}
+
+MapCell MapGrid::operator() (unsigned row, unsigned col) const {
+ //check for legal index
+ assert(row < rows_ && col < cols_);
+ return map_[cols_ * row + col];
+}
+
+
+MapGrid::MapGrid(const MapGrid& mg){
+ rows_ = mg.rows_;
+ cols_ = mg.cols_;
+ map_ = mg.map_;
+}
+
+MapGrid& MapGrid::operator= (const MapGrid& mg){
+ rows_ = mg.rows_;
+ cols_ = mg.cols_;
+ map_ = mg.map_;
+ return *this;
+}
Added: pkg/trunk/trajectory_rollout/src/tests.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/tests.cpp (rev 0)
+++ pkg/trunk/trajectory_rollout/src/tests.cpp 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,119 @@
+/*********************************************************************
+ * 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 <iostream>
+#include <vector>
+#include <utility>
+#include "map_cell.h"
+#include "map_grid.h"
+#include "trajectory_controller.h"
+
+using namespace std;
+
+//make sure that we are getting the path distance map expected
+bool testPathDistance(){
+ bool pass = true;
+ vector<pair<int, int> > path;
+
+ MapGrid mg(6, 6);
+
+ //create a path through the world
+ mg(2, 0).path_dist = 0.0;
+ mg(2, 1).path_dist = 0.0;
+ mg(3, 1).path_dist = 0.0;
+ mg(4, 1).path_dist = 0.0;
+ mg(4, 2).path_dist = 0.0;
+ mg(4, 3).path_dist = 0.0;
+ mg(3, 3).path_dist = 0.0;
+ mg(2, 3).path_dist = 0.0;
+ mg(1, 3).path_dist = 0.0;
+ mg(1, 4).path_dist = 0.0;
+ mg(1, 5).path_dist = 0.0;
+ mg(2, 5).path_dist = 0.0;
+ mg(3, 5).path_dist = 0.0;
+
+ //place some obstacles
+ mg(3,2).occ_state = 1;
+ mg(5,3).occ_state = 1;
+ mg(2,4).occ_state = 1;
+ mg(0,5).occ_state = 1;
+
+ //create a trajectory_controller
+ TrajectoryController tc(mg, path, 1, 1, 1);
+
+ tc.computePathDistance();
+
+ //print the results
+ cout.precision(2);
+ for(int k = tc.map_.rows_ - 1 ; k >= 0; --k){
+ for(unsigned int m = 0; m < tc.map_.cols_; ++m){
+ cout << tc.map_(k, m).path_dist << " | ";
+ }
+ cout << endl;
+ }
+ return pass;
+}
+
+//sanity check to make sure the grid functions correctly
+bool testGrid(){
+ bool pass = true;
+ MapGrid mg(10, 10);
+ MapCell mc;
+
+ for(int i = 0; i < 10; ++i){
+ for(int j = 0; j < 10; ++j){
+ mc.ci = i;
+ mc.cj = j;
+ mg(i, j) = mc;
+ }
+ }
+
+ for(int i = 0; i < 10; ++i){
+ for(int j = 0; j < 10; ++j){
+ if(!mg(i, j).ci == i || !mg(i, j).cj == j)
+ pass = false;
+ }
+ }
+ return pass;
+}
+
+//test some stuff
+int main(int argc, char** argv){
+ cout << "Test Grid: ";
+ testGrid() ? cout << "PASS" : cout << "FAIL";
+ cout << endl;
+
+ cout << "Test Path Distance: " << endl;
+ testPathDistance() ? cout << "PASS" : cout << "FAIL";
+ cout << endl;
+}
Added: pkg/trunk/trajectory_rollout/src/trajectory.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/trajectory.cpp (rev 0)
+++ pkg/trunk/trajectory_rollout/src/trajectory.cpp 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,46 @@
+/*********************************************************************
+ * 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"
+
+Trajectory::Trajectory(double xv, double yv, double thetav, double steps)
+ : xv_(xv), yv_(yv), thetav_(thetav)
+{
+ //size points to hold everything
+ points_.resize(steps);
+}
+
+void Trajectory::addPoint(int step, TrajectoryPoint tp){
+ points_[step] = tp;
+}
+
Added: pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp (rev 0)
+++ pkg/trunk/trajectory_rollout/src/trajectory_controller.cpp 2008-09-05 00:03:02 UTC (rev 3967)
@@ -0,0 +1,155 @@
+/*********************************************************************
+ * 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_controller.h"
+
+using namespace std;
+
+TrajectoryController::TrajectoryController(MapGrid mg, vector<pair<int, int> > path, double acc_x, double acc_y, double acc_theta)
+ : map_(mg), planned_path_(path), acc_x_(acc_x), acc_y_(acc_y), acc_theta_(acc_theta)
+{
+}
+
+//compute the distance from each cell in the map grid to the planned path
+void TrajectoryController::computePathDistance(){
+ //two sweeps are needed to compute path distance for the grid
+
+ //sweep bottom-top / left-right
+ for(unsigned int i = 0; i < map_.rows_; ++i)
+ for(unsigned int j = 0; j < map_.cols_; ++j)
+ updateNeighbors(&map_(i, j));
+
+ //sweep top-bottom / right-left
+ for(int i = map_.rows_ - 1; i >= 0; --i)
+ for(int j = map_.cols_ - 1; j >= 0; --j)
+ updateNeighbors(&map_(i, j));
+
+}
+
+//update neighboring path distance
+void TrajectoryController::updateNeighbors(MapCell* current){
+ //update the 6 neighbors of the current cell
+ for(int di = -1; di <= 1; ++di){
+ for(int dj = -1; dj <= 1; ++dj){
+ if(!(di == 0 && dj == 0))
+ cellPathDistance(current, di, dj);
+ }
+ }
+}
+
+//compute the distance from an individual cell to the planned path
+void TrajectoryController::cellPathDistance(MapCell* current, int di, int dj){
+ int ni = current->ci + di;
+ int nj = current->cj + dj;
+
+ //determine whether to add 1 to distance or 1.5 (for corner neighbors)
+ double new_dist = current->path_dist + sqrt( di * di + dj * dj);
+
+ //make sure we are looking at a cell that exists
+ if(!VALID_CELL(map_, ni, nj))
+ return;
+
+ //only modify the neighboring cell if its distance from the path will shrink
+ //also, do nothing with cells that contain obstacles
+ if(map_(ni, nj).occ_state == 1 || map_(ni, nj).path_dist < new_dist){
+ return;
+ }
+
+ //update path distance
+ map_(ni, nj).path_dist = new_dist;
+}
+
+//create a trajectory given the current pose of the robot and selected velocities
+void TrajectoryController::generateTrajectory(double x, double y, double theta, double vx, double vy,
+ double vtheta, double num_steps, double sim_time){
+ double x_i = x;
+ double y_i = y;
+ double theta_i = theta;
+ double vx_i = vx;
+ double vy_i = vy;
+ double vtheta_i = vtheta;
+ double dt = sim_time / num_steps;
+
+ Trajectory traj(vx, vy, vtheta, num_steps);
+ for(int i = 0; i < num_steps; ++i){
+ traj.addPoint(i, TrajectoryPoint(i * dt, x_i, y_i, theta_i, vx_i, vy_i, vtheta_i));
+
+ //calculate velocities
+ vx_i = computeNewVelocity(vx, vx_i, acc_x_, dt);
+ vy_i = computeNewVelocity(vy, vy_i, acc_y_, dt);
+ vtheta_i = computeNewVelocity(vtheta, vtheta_i, acc_theta_, dt);
+
+ //calculate positions
+ x_i = computeNewPosition(x_i, vx_i, dt);
+ y_i = computeNewPosition(y_i, vy_i, dt);
+ theta_i = computeNewPosition(theta_i, vtheta_i, dt);
+
+ }
+}
+
+//compute position based on velocity
+double TrajectoryController::computeNewPosition(double xi, double v, double dt){
+ return xi + v * dt;
+}
+
+//compute velocity based on acceleration
+double TrajectoryController::computeNewVelocity(double vg, double vi, double a_max, double dt){
+ if(vg >= 0)
+ return MIN(vg, vi + a_max * dt);
+ return MAX(vg, vi + a_max * dt);
+}
+
+
+/*TODO: Implement this for real
+void TrajectoryController::findPath(){
+ //compute feasible velocity limits
+ double max_vel_x = vel_x_ + acc_x_;
+ double min_vel_x = vel_x_ - acc_x_;
+
+ double max_vel_y = vel_y_ + acc_y_;
+ double min_vel_y = vel_y_ - acc_y_;
+
+ double max_vel_theta = vel_theta_ + acc_theta_;
+ double min_vel_theta = vel_theta_ - acc_theta_;
+}
+
+void TrajectoryController::sampleSpace(){
+ srand((unsigned) time(0));
+ int random_int;
+ int highest, lowest;
+
+ for(int i = 0; i < num_samples; ++i){
+ rand_int = lowest + int(range * rand() / RAND_MAX + 1.0);
+ }
+}
+*/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
