Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14130] pkg/trunk
|
From: <ge...@us...> - 2009-04-21 04:29:51
|
Revision: 14130
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14130&view=rev
Author: gerkey
Date: 2009-04-21 04:29:39 +0000 (Tue, 21 Apr 2009)
Log Message:
-----------
Moved wavefront_player to wavefront.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
Added Paths:
-----------
pkg/trunk/nav/wavefront/
pkg/trunk/nav/wavefront/CMakeLists.txt
pkg/trunk/nav/wavefront/Makefile
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/heap.c
pkg/trunk/nav/wavefront/src/heap.h
pkg/trunk/nav/wavefront/src/plan.c
pkg/trunk/nav/wavefront/src/plan.h
pkg/trunk/nav/wavefront/src/plan_control.c
pkg/trunk/nav/wavefront/src/plan_plan.c
pkg/trunk/nav/wavefront/src/plan_waypoint.c
pkg/trunk/nav/wavefront/src/test.c
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Removed Paths:
-------------
pkg/trunk/nav/wavefront_player/
Modified: pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_erratic/2dnav_erratic.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -2,7 +2,7 @@
<group name="wg">
<include file="$(find 2dnav_erratic)/lowlevel.xml"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen" />
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_erratic/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -8,6 +8,6 @@
<depend package="erratic_player"/>
<depend package="sicktoolbox_wrapper"/>
<depend package="map_server"/>
- <depend package="wavefront_player"/>
+ <depend package="wavefront"/>
<depend package="highlevel_controllers"/>
</package>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -12,7 +12,6 @@
<depend package="map_server"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="wavefront_player"/>
<depend package="teleop_base"/>
<depend package="teleop_arm_keyboard"/>
<depend package="highlevel_controllers"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,7 +3,7 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_5cm.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,7 +3,7 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full-0.05.pgm 0.05" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <node pkg="wavefront" type="wavefront" respawn="false" >
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_wavefront_multirobot.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,13 +3,13 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2-multi.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" />
- <param name="robot_0/wavefront_player_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_0" ns="robot_0" output="screen">
+ <param name="robot_0/wavefront_0/tf_prefix" value="robot_0" /><!--Broken see ros ticket:941 replaced by above-->
+ <node pkg="wavefront" type="wavefront" respawn="false" name="wavefront_0" ns="robot_0" output="screen">
<param name="tf_prefix" value="robot_0"/>
<remap from="scan" to="base_scan" />
</node>
- <param name="robot_1/wavefront_player_1/tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" name="wavefront_player_1" ns="robot_1" >
+ <param name="robot_1/wavefront_1/tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
+ <node pkg="wavefront" type="wavefront" respawn="false" name="wavefront_1" ns="robot_1" >
<param name="tf_prefix" value="robot_1" /><!--Broken see ros ticket:941 replaced by above-->
<remap from="scan" to="base_scan" />
</node>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -3,24 +3,24 @@
<group name="wg">
<node pkg="stage" type="stageros" args="$(find 2dnav_stage)/worlds/willow-pr2.world" respawn="false" output="screen"/>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-full.pgm 0.1" respawn="false" output="screen"/>
- <param name="pf_laser_max_beams" value="6"/>
+ <param name="pf_laser_max_beams" value="20"/>
<param name="pf_min_samples" value="100"/>
<param name="pf_max_samples" value="1000"/>
- <param name="pf_odom_drift_xx" value="0.1"/>
- <param name="pf_odom_drift_yy" value="0.1"/>
- <param name="pf_odom_drift_aa" value="0.1"/>
- <param name="pf_odom_drift_xa" value="0.1"/>
+ <param name="pf_odom_drift_xx" value="0.5"/>
+ <param name="pf_odom_drift_yy" value="0.5"/>
+ <param name="pf_odom_drift_aa" value="0.5"/>
+ <param name="pf_odom_drift_xa" value="0.5"/>
<param name="pf_min_d" value="0.25"/> <!-- 25cm -->
<param name="pf_min_a" value="0.349"/> <!-- 20 degrees -->
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
+ <node pkg="amcl_player" type="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
- <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+ <!--node pkg="nav_view" type="nav_view" respawn="false" output="screen"/-->
+ <node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/>
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_2.5cm.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -16,7 +16,7 @@
<remap from="scan" to="base_scan" />
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_wavefront_newamcl.launch 2009-04-21 04:29:39 UTC (rev 14130)
@@ -35,7 +35,7 @@
<param name="robot_th_start" value="0.0"/>
</node>
<param name="robot_radius" value="0.325"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <node pkg="wavefront" type="wavefront" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
</node>
<node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -12,5 +12,5 @@
<depend package="fake_localization"/>
<depend package="stage"/>
<depend package="map_server"/>
- <depend package="wavefront_player"/>
+ <depend package="wavefront"/>
</package>
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-04-21 04:12:37 UTC (rev 14129)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -9,7 +9,6 @@
<depend package="map_server"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
- <depend package="wavefront_player"/>
<depend package="teleop_arm_keyboard"/>
<depend package="highlevel_controllers"/>
<depend package="executive_trex_pr2"/>
Added: pkg/trunk/nav/wavefront/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/wavefront/CMakeLists.txt (rev 0)
+++ pkg/trunk/nav/wavefront/CMakeLists.txt 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(wavefront)
+
+rospack_add_boost_directories()
+
+rospack_add_library(wavefront_planner
+ src/heap.c
+ src/plan.c
+ src/plan_control.c
+ src/plan_plan.c
+ src/plan_waypoint.c)
+
+rospack_add_executable(bin/wavefront src/wavefront_node.cpp)
+target_link_libraries(bin/wavefront wavefront_planner)
+rospack_link_boost(bin/wavefront thread)
+
+# this little program just lets you send and block on goals from the shell
+rospack_add_executable(bin/cli src/cli.cpp)
+rospack_link_boost(bin/cli thread)
+
+# TODO: Make this program into a real unit test, and wean it off GDK
+#rospack_add_executable(bin/test src/test.c)
+#target_link_libraries(bin/test wavefront_planner)
Added: pkg/trunk/nav/wavefront/Makefile
===================================================================
--- pkg/trunk/nav/wavefront/Makefile (rev 0)
+++ pkg/trunk/nav/wavefront/Makefile 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml (rev 0)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,12 @@
+<package>
+ <description>A ROS node that uses the Player libwavefront_standalone library, which implements wavefront path-planning for a planar robot.</description>
+ <author>Brian P. Gerkey</author>
+ <license>LGPL</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="laser_scan" />
+ <depend package="robot_srvs" />
+ <depend package="robot_msgs" />
+ <depend package="robot_actions" />
+ <depend package="tf"/>
+</package>
Added: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp (rev 0)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,70 @@
+#include <cstdio>
+#include <stdlib.h>
+#include "ros/node.h"
+#include "ros/publisher.h"
+#include "robot_msgs/Planner2DState.h"
+#include "robot_msgs/Planner2DGoal.h"
+
+class WavefrontCLI : public ros::Node
+{
+public:
+ robot_msgs::Planner2DState wf_state;
+ robot_msgs::Planner2DGoal wf_goal;
+ enum { WF_IDLE, WF_SEEKING_GOAL, WF_DONE } state;
+
+ WavefrontCLI(double x, double y, double th)
+ : ros::Node("wavefront_cli"), state(WF_IDLE)
+ {
+ wf_goal.goal.x = x;
+ wf_goal.goal.y = y;
+ wf_goal.goal.th = th;
+ wf_goal.enable = 1;
+ subscribe("state", wf_state, &WavefrontCLI::state_cb, 1);
+ advertise("goal", wf_goal, &WavefrontCLI::goal_subscriber_callback, 1);
+ }
+ void state_cb()
+ {
+ if (state == WF_IDLE && wf_state.status.value == wf_state.status.ACTIVE)
+ state = WF_SEEKING_GOAL;
+ else if (state == WF_SEEKING_GOAL && wf_state.status.value != wf_state.status.ACTIVE)
+ state = WF_DONE;
+ }
+ void deactivate_goal()
+ {
+ wf_goal.enable = 0;
+ publish("goal", wf_goal);
+ ros::Duration().fromSec(0.5).sleep(); // hack to try and wait for the message to go
+ }
+ void goal_subscriber_callback(const ros::PublisherPtr& pub)
+ {
+ publish("goal", wf_goal);
+ }
+};
+
+int main(int argc, char **argv)
+{
+ if (argc != 5)
+ {
+ printf("usage: ./cli X Y THETA TIMEOUT\n"
+ " where X and Y are in meters,\n"
+ " THETA is in radians,\n"
+ " and TIMEOUT is in seconds.\n");
+ return 1;
+ }
+ ros::init(argc, argv);
+ double max_secs = atof(argv[4]);
+ WavefrontCLI wf_cli(atof(argv[1]), atof(argv[2]), atof(argv[3]));
+ ros::Time t_start(ros::Time::now());
+ while (wf_cli.ok() && wf_cli.state != WavefrontCLI::WF_DONE &&
+ ros::Time::now() - t_start < ros::Duration().fromSec(max_secs))
+ ros::Duration().fromSec(0.1).sleep();
+ if (wf_cli.ok() && wf_cli.state != WavefrontCLI::WF_DONE) // didn't get there
+ {
+ printf("timeout\n");
+ wf_cli.deactivate_goal();
+ }
+ else
+ printf("success\n");
+
+ return 0;
+}
Added: pkg/trunk/nav/wavefront/src/heap.c
===================================================================
--- pkg/trunk/nav/wavefront/src/heap.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/heap.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,166 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey ge...@wi...
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+#include <stdlib.h>
+#include <assert.h>
+#include <stdio.h>
+
+#include "heap.h"
+
+heap_t*
+heap_alloc(int size, heap_free_elt_fn_t free_fn)
+{
+ heap_t* h;
+
+ h = calloc(1,sizeof(heap_t));
+ assert(h);
+ h->size = size;
+ h->free_fn = free_fn;
+ h->A = calloc(h->size,sizeof(double));
+ assert(h->A);
+ h->data = calloc(h->size,sizeof(void*));
+ assert(h->data);
+ h->len = 0;
+
+ return(h);
+}
+
+void
+heap_free(heap_t* h)
+{
+ if(h->free_fn)
+ {
+ while(!heap_empty(h))
+ (*h->free_fn)(heap_extract_max(h));
+ }
+ free(h->data);
+ free(h->A);
+ free(h);
+}
+
+void
+heap_heapify(heap_t* h, int i)
+{
+ int l, r;
+ int largest;
+ double tmp;
+ void* tmp_data;
+
+ l = HEAP_LEFT(i);
+ r = HEAP_RIGHT(i);
+
+ if((l < h->len) && (h->A[l] > h->A[i]))
+ largest = l;
+ else
+ largest = i;
+
+ if((r < h->len) && (h->A[r] > h->A[largest]))
+ largest = r;
+
+ if(largest != i)
+ {
+ tmp = h->A[i];
+ tmp_data = h->data[i];
+ h->A[i] = h->A[largest];
+ h->data[i] = h->data[largest];
+ h->A[largest] = tmp;
+ h->data[largest] = tmp_data;
+ heap_heapify(h,largest);
+ }
+}
+
+int
+heap_empty(heap_t* h)
+{
+ return(h->len == 0);
+}
+
+void*
+heap_extract_max(heap_t* h)
+{
+ void* max;
+
+ assert(h->len > 0);
+
+ max = h->data[0];
+ h->A[0] = h->A[h->len - 1];
+ h->data[0] = h->data[h->len - 1];
+ h->len--;
+ heap_heapify(h,0);
+ return(max);
+}
+
+void
+heap_insert(heap_t* h, double key, void* data)
+{
+ int i;
+
+ if(h->len == h->size)
+ {
+ h->size *= 2;
+ h->A = realloc(h->A, h->size * sizeof(double));
+ assert(h->A);
+ h->data = realloc(h->data, h->size * sizeof(void*));
+ assert(h->data);
+ }
+
+ h->len++;
+ i = h->len - 1;
+
+ while((i > 0) && (h->A[HEAP_PARENT(i)] < key))
+ {
+ h->A[i] = h->A[HEAP_PARENT(i)];
+ h->data[i] = h->data[HEAP_PARENT(i)];
+ i = HEAP_PARENT(i);
+ }
+ h->A[i] = key;
+ h->data[i] = data;
+}
+
+int
+heap_valid(heap_t* h)
+{
+ int i;
+ for(i=1;i<h->len;i++)
+ {
+ if(h->A[HEAP_PARENT(i)] < h->A[i])
+ return(0);
+ }
+ return(1);
+}
+
+void
+heap_reset(heap_t* h)
+{
+ h->len = 0;
+}
+
+void
+heap_dump(heap_t* h)
+{
+ int i;
+ for(i=0;i<h->len;i++)
+ printf("%d: %f\n", i, h->A[i]);
+}
Added: pkg/trunk/nav/wavefront/src/heap.h
===================================================================
--- pkg/trunk/nav/wavefront/src/heap.h (rev 0)
+++ pkg/trunk/nav/wavefront/src/heap.h 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,65 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey ge...@wi...
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/*
+ * An implementation of a heap, as seen in "Introduction to Algorithms," by
+ * Cormen, Leiserson, and Rivest, pages 140-152.
+ */
+
+#ifndef _HEAP_H_
+#define _HEAP_H_
+
+#define HEAP_PARENT(i) ((i)/2)
+#define HEAP_LEFT(i) (2*(i))
+#define HEAP_RIGHT(i) (2*(i)+1)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct heap;
+
+typedef void (*heap_free_elt_fn_t) (void* elt);
+
+typedef struct heap
+{
+ int len;
+ int size;
+ heap_free_elt_fn_t free_fn;
+ double* A;
+ void** data;
+} heap_t;
+
+heap_t* heap_alloc(int size, heap_free_elt_fn_t free_fn);
+void heap_free(heap_t* h);
+void heap_heapify(heap_t* h, int i);
+void* heap_extract_max(heap_t* h);
+void heap_insert(heap_t* h, double key, void* data);
+void heap_dump(heap_t* h);
+int heap_valid(heap_t* h);
+int heap_empty(heap_t* h);
+void heap_reset(heap_t* h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Added: pkg/trunk/nav/wavefront/src/plan.c
===================================================================
--- pkg/trunk/nav/wavefront/src/plan.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/plan.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,757 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Andrew Howard
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**************************************************************************
+ * Desc: Path planning
+ * Author: Andrew Howard
+ * Date: 10 Oct 2002
+ * CVS: $Id: plan.c 6591 2008-06-16 21:20:10Z gerkey $
+**************************************************************************/
+
+//#include <config.h>
+
+#ifndef MIN
+ #define MIN(a,b) ((a < b) ? (a) : (b))
+#endif
+#ifndef MAX
+ #define MAX(a,b) ((a > b) ? (a) : (b))
+#endif
+
+// This header MUST come before <openssl/md5.h>
+#include <sys/types.h>
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+ #include <openssl/md5.h>
+#endif
+
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+
+//#include <libplayercore/playercommon.h>
+//#include <libplayercore/error.h>
+
+#include "plan.h"
+//#include "heap.h"
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+// length of the hash, in unsigned ints
+#define HASH_LEN (MD5_DIGEST_LENGTH / sizeof(unsigned int))
+#endif
+
+#include <sys/time.h>
+static double get_time(void);
+
+#if 0
+void draw_cspace(plan_t* plan, const char* fname);
+#endif
+
+// Create a planner
+plan_t *plan_alloc(double abs_min_radius, double des_min_radius,
+ double max_radius, double dist_penalty,
+ double hysteresis_factor)
+{
+ plan_t *plan;
+
+ plan = calloc(1, sizeof(plan_t));
+
+ plan->abs_min_radius = abs_min_radius;
+ plan->des_min_radius = des_min_radius;
+
+ plan->max_radius = max_radius;
+ plan->dist_penalty = dist_penalty;
+ plan->hysteresis_factor = hysteresis_factor;
+
+ plan->heap = heap_alloc(PLAN_DEFAULT_HEAP_SIZE, (heap_free_elt_fn_t)NULL);
+ assert(plan->heap);
+
+ plan->path_size = 1000;
+ plan->path = calloc(plan->path_size, sizeof(plan->path[0]));
+
+ plan->lpath_size = 100;
+ plan->lpath = calloc(plan->lpath_size, sizeof(plan->lpath[0]));
+
+ plan->waypoint_size = 100;
+ plan->waypoints = calloc(plan->waypoint_size, sizeof(plan->waypoints[0]));
+
+ return plan;
+}
+
+void
+plan_set_obstacles(plan_t* plan, double* obs, size_t num)
+{
+ size_t i;
+ int j;
+ int di,dj;
+ float* p;
+ plan_cell_t* cell, *ncell;
+ double t0,t1;
+
+ t0 = get_time();
+
+ // Start with static obstacle data
+ cell = plan->cells;
+ for(j=0;j<plan->size_y*plan->size_x;j++,cell++)
+ {
+ cell->occ_state_dyn = cell->occ_state;
+ cell->occ_dist_dyn = cell->occ_dist;
+ cell->mark = 0;
+ }
+
+ // Expand around the dynamic obstacle pts
+ for(i=0;i<num;i++)
+ {
+ // Convert to grid coords
+ int gx,gy;
+ gx = PLAN_GXWX(plan, obs[2*i]);
+ gy = PLAN_GYWY(plan, obs[2*i+1]);
+
+ if(!PLAN_VALID(plan,gx,gy))
+ continue;
+
+ cell = plan->cells + PLAN_INDEX(plan,gx,gy);
+
+ if(cell->mark)
+ continue;
+
+ cell->mark = 1;
+ cell->occ_state_dyn = 1;
+ cell->occ_dist_dyn = 0.0;
+
+ p = plan->dist_kernel;
+ for (dj = -plan->dist_kernel_width/2;
+ dj <= plan->dist_kernel_width/2;
+ dj++)
+ {
+ ncell = cell + -plan->dist_kernel_width/2 + dj*plan->size_x;
+ for (di = -plan->dist_kernel_width/2;
+ di <= plan->dist_kernel_width/2;
+ di++, p++, ncell++)
+ {
+ if(!PLAN_VALID_BOUNDS(plan,cell->ci+di,cell->cj+dj))
+ continue;
+
+ if(*p < ncell->occ_dist_dyn)
+ ncell->occ_dist_dyn = *p;
+ }
+ }
+ }
+
+ t1 = get_time();
+ //printf("plan_set_obstacles: %.6lf\n", t1-t0);
+}
+
+void
+plan_compute_dist_kernel(plan_t* plan)
+{
+ int i,j;
+ float* p;
+
+ // Compute variable sized kernel, for use in propagating distance from
+ // obstacles
+ plan->dist_kernel_width = 1 + 2 * (int)ceil(plan->max_radius / plan->scale);
+ plan->dist_kernel = (float*)realloc(plan->dist_kernel,
+ sizeof(float) *
+ plan->dist_kernel_width *
+ plan->dist_kernel_width);
+ assert(plan->dist_kernel);
+
+ p = plan->dist_kernel;
+ for(j=-plan->dist_kernel_width/2;j<=plan->dist_kernel_width/2;j++)
+ {
+ for(i=-plan->dist_kernel_width/2;i<=plan->dist_kernel_width/2;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ }
+ }
+ // also compute a 3x3 kernel, used when propagating distance from goal
+ p = plan->dist_kernel_3x3;
+ for(j=-1;j<=1;j++)
+ {
+ for(i=-1;i<=1;i++,p++)
+ {
+ *p = sqrt(i*i+j*j) * plan->scale;
+ }
+ }
+}
+
+
+// Destroy a planner
+void plan_free(plan_t *plan)
+{
+ if (plan->cells)
+ free(plan->cells);
+ heap_free(plan->heap);
+ free(plan->waypoints);
+ if(plan->dist_kernel)
+ free(plan->dist_kernel);
+ free(plan);
+
+ return;
+}
+
+// Initialize the plan
+void plan_init(plan_t *plan)
+{
+ int i, j;
+ plan_cell_t *cell;
+
+ printf("scale: %.3lf\n", plan->scale);
+
+ cell = plan->cells;
+ for (j = 0; j < plan->size_y; j++)
+ {
+ for (i = 0; i < plan->size_x; i++, cell++)
+ {
+ cell->ci = i;
+ cell->cj = j;
+ cell->occ_state_dyn = cell->occ_state;
+ if(cell->occ_state >= 0)
+ cell->occ_dist_dyn = cell->occ_dist = 0.0;
+ else
+ cell->occ_dist_dyn = cell->occ_dist = plan->max_radius;
+ cell->plan_cost = PLAN_MAX_COST;
+ cell->plan_next = NULL;
+ cell->lpathmark = 0;
+ }
+ }
+ plan->waypoint_count = 0;
+
+ plan_compute_dist_kernel(plan);
+
+ plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
+}
+
+
+// Reset the plan
+void plan_reset(plan_t *plan)
+{
+ int i, j;
+ plan_cell_t *cell;
+
+ for (j = plan->min_y; j <= plan->max_y; j++)
+ {
+ for (i = plan->min_x; i <= plan->max_x; i++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan,i,j);
+ cell->plan_cost = PLAN_MAX_COST;
+ cell->plan_next = NULL;
+ cell->mark = 0;
+ }
+ }
+ plan->waypoint_count = 0;
+}
+
+void
+plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y)
+{
+ min_x = MAX(0,min_x);
+ min_x = MIN(plan->size_x-1, min_x);
+ min_y = MAX(0,min_y);
+ min_y = MIN(plan->size_y-1, min_y);
+ max_x = MAX(0,max_x);
+ max_x = MIN(plan->size_x-1, max_x);
+ max_y = MAX(0,max_y);
+ max_y = MIN(plan->size_y-1, max_y);
+
+ assert(min_x <= max_x);
+ assert(min_y <= max_y);
+
+ plan->min_x = min_x;
+ plan->min_y = min_y;
+ plan->max_x = max_x;
+ plan->max_y = max_y;
+
+ //printf("new bounds: (%d,%d) -> (%d,%d)\n",
+ //plan->min_x, plan->min_y,
+ //plan->max_x, plan->max_y);
+}
+
+int
+plan_check_inbounds(plan_t* plan, double x, double y)
+{
+ int gx, gy;
+
+ gx = PLAN_GXWX(plan, x);
+ gy = PLAN_GYWY(plan, y);
+
+ if((gx >= plan->min_x) && (gx <= plan->max_x) &&
+ (gy >= plan->min_y) && (gy <= plan->max_y))
+ return(1);
+ else
+ return(0);
+}
+
+void
+plan_set_bbox(plan_t* plan, double padding, double min_size,
+ double x0, double y0, double x1, double y1)
+{
+ int gx0, gy0, gx1, gy1;
+ int min_x, min_y, max_x, max_y;
+ int sx, sy;
+ int dx, dy;
+ int gmin_size;
+ int gpadding;
+
+ gx0 = PLAN_GXWX(plan, x0);
+ gy0 = PLAN_GYWY(plan, y0);
+ gx1 = PLAN_GXWX(plan, x1);
+ gy1 = PLAN_GYWY(plan, y1);
+
+ // Make a bounding box to include both points.
+ min_x = MIN(gx0, gx1);
+ min_y = MIN(gy0, gy1);
+ max_x = MAX(gx0, gx1);
+ max_y = MAX(gy0, gy1);
+
+ // Make sure the min_size is achievable
+ gmin_size = (int)ceil(min_size / plan->scale);
+ gmin_size = MIN(gmin_size, MIN(plan->size_x-1, plan->size_y-1));
+
+ // Add padding
+ gpadding = (int)ceil(padding / plan->scale);
+ min_x -= gpadding / 2;
+ min_x = MAX(min_x, 0);
+ max_x += gpadding / 2;
+ max_x = MIN(max_x, plan->size_x - 1);
+ min_y -= gpadding / 2;
+ min_y = MAX(min_y, 0);
+ max_y += gpadding / 2;
+ max_y = MIN(max_y, plan->size_y - 1);
+
+ // Grow the box if necessary to achieve the min_size
+ sx = max_x - min_x;
+ while(sx < gmin_size)
+ {
+ dx = gmin_size - sx;
+ min_x -= (int)ceil(dx / 2.0);
+ max_x += (int)ceil(dx / 2.0);
+
+ min_x = MAX(min_x, 0);
+ max_x = MIN(max_x, plan->size_x-1);
+
+ sx = max_x - min_x;
+ }
+ sy = max_y - min_y;
+ while(sy < gmin_size)
+ {
+ dy = gmin_size - sy;
+ min_y -= (int)ceil(dy / 2.0);
+ max_y += (int)ceil(dy / 2.0);
+
+ min_y = MAX(min_y, 0);
+ max_y = MIN(max_y, plan->size_y-1);
+
+ sy = max_y - min_y;
+ }
+
+ plan_set_bounds(plan, min_x, min_y, max_x, max_y);
+}
+
+void
+plan_compute_cspace(plan_t* plan)
+{
+ int i, j;
+ int di, dj;
+ float* p;
+ plan_cell_t *cell, *ncell;
+
+ puts("Generating C-space....");
+
+ for (j = plan->min_y; j <= plan->max_y; j++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan, 0, j);
+ for (i = plan->min_x; i <= plan->max_x; i++, cell++)
+ {
+ if (cell->occ_state < 0)
+ continue;
+
+ p = plan->dist_kernel;
+ for (dj = -plan->dist_kernel_width/2;
+ dj <= plan->dist_kernel_width/2;
+ dj++)
+ {
+ ncell = cell + -plan->dist_kernel_width/2 + dj*plan->size_x;
+ for (di = -plan->dist_kernel_width/2;
+ di <= plan->dist_kernel_width/2;
+ di++, p++, ncell++)
+ {
+ if(!PLAN_VALID_BOUNDS(plan,i+di,j+dj))
+ continue;
+
+ if(*p < ncell->occ_dist)
+ ncell->occ_dist_dyn = ncell->occ_dist = *p;
+ }
+ }
+ }
+ }
+}
+
+#if 0
+#include <gdk-pixbuf/gdk-pixbuf.h>
+
+void
+draw_cspace(plan_t* plan, const char* fname)
+{
+ GdkPixbuf* pixbuf;
+ GError* error = NULL;
+ guchar* pixels;
+ int p;
+ int paddr;
+ int i, j;
+
+ pixels = (guchar*)malloc(sizeof(guchar)*plan->size_x*plan->size_y*3);
+
+ p=0;
+ for(j=plan->size_y-1;j>=0;j--)
+ {
+ for(i=0;i<plan->size_x;i++,p++)
+ {
+ paddr = p * 3;
+ //if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1)
+ if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->abs_min_radius)
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 0;
+ }
+ else if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->max_radius)
+ {
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ else
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 255;
+ }
+ }
+ }
+
+ pixbuf = gdk_pixbuf_new_from_data(pixels,
+ GDK_COLORSPACE_RGB,
+ 0,8,
+ plan->size_x,
+ plan->size_y,
+ plan->size_x * 3,
+ NULL, NULL);
+
+ gdk_pixbuf_save(pixbuf,fname,"png",&error,NULL);
+ gdk_pixbuf_unref(pixbuf);
+ free(pixels);
+}
+
+ void
+draw_path(plan_t* plan, double lx, double ly, const char* fname)
+{
+ GdkPixbuf* pixbuf;
+ GError* error = NULL;
+ guchar* pixels;
+ int p;
+ int paddr;
+ int i, j;
+ plan_cell_t* cell;
+
+ pixels = (guchar*)malloc(sizeof(guchar)*plan->size_x*plan->size_y*3);
+
+ p=0;
+ for(j=plan->size_y-1;j>=0;j--)
+ {
+ for(i=0;i<plan->size_x;i++,p++)
+ {
+ paddr = p * 3;
+ if(plan->cells[PLAN_INDEX(plan,i,j)].occ_state == 1)
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 0;
+ }
+ else if(plan->cells[PLAN_INDEX(plan,i,j)].occ_dist < plan->max_radius)
+ {
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ else
+ {
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 255;
+ }
+ /*
+ if((7*plan->cells[PLAN_INDEX(plan,i,j)].plan_cost) > 255)
+ {
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ else
+ {
+ pixels[paddr] = 255 - 7*plan->cells[PLAN_INDEX(plan,i,j)].plan_cost;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 0;
+ }
+ */
+ }
+ }
+
+ for(i=0;i<plan->path_count;i++)
+ {
+ cell = plan->path[i];
+
+ paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
+ pixels[paddr] = 0;
+ pixels[paddr+1] = 255;
+ pixels[paddr+2] = 0;
+ }
+
+ for(i=0;i<plan->lpath_count;i++)
+ {
+ cell = plan->lpath[i];
+
+ paddr = 3*PLAN_INDEX(plan,cell->ci,plan->size_y - cell->cj - 1);
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+
+ /*
+ for(p=0;p<plan->waypoint_count;p++)
+ {
+ cell = plan->waypoints[p];
+ for(j=-3;j<=3;j++)
+ {
+ cj = cell->cj + j;
+ for(i=-3;i<=3;i++)
+ {
+ ci = cell->ci + i;
+ paddr = 3*PLAN_INDEX(plan,ci,plan->size_y - cj - 1);
+ pixels[paddr] = 255;
+ pixels[paddr+1] = 0;
+ pixels[paddr+2] = 255;
+ }
+ }
+ }
+ */
+
+ pixbuf = gdk_pixbuf_new_from_data(pixels,
+ GDK_COLORSPACE_RGB,
+ 0,8,
+ plan->size_x,
+ plan->size_y,
+ plan->size_x * 3,
+ NULL, NULL);
+
+ gdk_pixbuf_save(pixbuf,fname,"png",&error,NULL);
+ gdk_pixbuf_unref(pixbuf);
+ free(pixels);
+}
+#endif
+
+// Construct the configuration space from the occupancy grid.
+// This treats both occupied and unknown cells as bad.
+//
+// If cachefile is non-NULL, then we try to read the c-space from that
+// file. If that fails, then we construct the c-space as per normal and
+// then write it out to cachefile.
+#if 0
+void
+plan_update_cspace(plan_t *plan, const char* cachefile)
+{
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+ unsigned int hash[HASH_LEN];
+ plan_md5(hash, plan);
+ if(cachefile && strlen(cachefile))
+ {
+ PLAYER_MSG1(2,"Trying to read c-space from file %s", cachefile);
+ if(plan_read_cspace(plan,cachefile,hash) == 0)
+ {
+ // Reading from the cache file worked; we're done here.
+ PLAYER_MSG1(2,"Successfully read c-space from file %s", cachefile);
+#if 0
+ draw_cspace(plan,"plan_cspace.png");
+#endif
+ return;
+ }
+ PLAYER_MSG1(2, "Failed to read c-space from file %s", cachefile);
+ }
+#endif
+
+ //plan_update_cspace_dp(plan);
+ plan_update_cspace_naive(plan);
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+ if(cachefile)
+ plan_write_cspace(plan,cachefile, (unsigned int*)hash);
+#endif
+
+ PLAYER_MSG0(2,"Done.");
+
+#if 0
+ draw_cspace(plan,"plan_cspace.png");
+#endif
+}
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+// Write the cspace occupancy distance values to a file, one per line.
+// Read them back in with plan_read_cspace().
+// Returns non-zero on error.
+int
+plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash)
+{
+ plan_cell_t* cell;
+ int i,j;
+ FILE* fp;
+
+ if(!(fp = fopen(fname,"w+")))
+ {
+ PLAYER_MSG2(2,"Failed to open file %s to write c-space: %s",
+ fname,strerror(errno));
+ return(-1);
+ }
+
+ fprintf(fp,"%d\n%d\n", plan->size_x, plan->size_y);
+ fprintf(fp,"%.3lf\n%.3lf\n", plan->origin_x, plan->origin_y);
+ fprintf(fp,"%.3lf\n%.3lf\n", plan->scale,plan->max_radius);
+ for(i=0;i<HASH_LEN;i++)
+ fprintf(fp,"%08X", hash[i]);
+ fprintf(fp,"\n");
+
+ for(j = 0; j < plan->size_y; j++)
+ {
+ for(i = 0; i < plan->size_x; i++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan, i, j);
+ fprintf(fp,"%.3f\n", cell->occ_dist);
+ }
+ }
+
+ fclose(fp);
+ return(0);
+}
+
+// Read the cspace occupancy distance values from a file, one per line.
+// Write them in first with plan_read_cspace().
+// Returns non-zero on error.
+int
+plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash)
+{
+ plan_cell_t* cell;
+ int i,j;
+ FILE* fp;
+ int size_x, size_y;
+ double origin_x, origin_y;
+ double scale, max_radius;
+ unsigned int cached_hash[HASH_LEN];
+
+ if(!(fp = fopen(fname,"r")))
+ {
+ PLAYER_MSG1(2,"Failed to open file %s", fname);
+ return(-1);
+ }
+
+ /* Read out the metadata */
+ if((fscanf(fp,"%d", &size_x) < 1) ||
+ (fscanf(fp,"%d", &size_y) < 1) ||
+ (fscanf(fp,"%lf", &origin_x) < 1) ||
+ (fscanf(fp,"%lf", &origin_y) < 1) ||
+ (fscanf(fp,"%lf", &scale) < 1) ||
+ (fscanf(fp,"%lf", &max_radius) < 1))
+ {
+ PLAYER_MSG1(2,"Failed to read c-space metadata from file %s", fname);
+ fclose(fp);
+ return(-1);
+ }
+
+ for(i=0;i<HASH_LEN;i++)
+ {
+ if(fscanf(fp,"%08X", cached_hash+i) < 1)
+ {
+ PLAYER_MSG1(2,"Failed to read c-space metadata from file %s", fname);
+ fclose(fp);
+ return(-1);
+ }
+ }
+
+ /* Verify that metadata matches */
+ if((size_x != plan->size_x) ||
+ (size_y != plan->size_y) ||
+ (fabs(origin_x - plan->origin_x) > 1e-3) ||
+ (fabs(origin_y - plan->origin_y) > 1e-3) ||
+ (fabs(scale - plan->scale) > 1e-3) ||
+ (fabs(max_radius - plan->max_radius) > 1e-3) ||
+ memcmp(cached_hash, hash, sizeof(unsigned int) * HASH_LEN))
+ {
+ PLAYER_MSG1(2,"Mismatch in c-space metadata read from file %s", fname);
+ fclose(fp);
+ return(-1);
+ }
+
+ for(j = 0; j < plan->size_y; j++)
+ {
+ for(i = 0; i < plan->size_x; i++)
+ {
+ cell = plan->cells + PLAN_INDEX(plan, i, j);
+ if(fscanf(fp,"%f", &(cell->occ_dist)) < 1)
+ {
+ PLAYER_MSG3(2,"Failed to read c-space data for cell (%d,%d) from file %s",
+ i,j,fname);
+ fclose(fp);
+ return(-1);
+ }
+ }
+ }
+
+ fclose(fp);
+ return(0);
+}
+
+// Compute the 16-byte MD5 hash of the map data in the given plan
+// object.
+void
+plan_md5(unsigned int* digest, plan_t* plan)
+{
+ MD5_CTX c;
+
+ MD5_Init(&c);
+
+ MD5_Update(&c,(const unsigned char*)plan->cells,
+ (plan->size_x*plan->size_y)*sizeof(plan_cell_t));
+
+ MD5_Final((unsigned char*)digest,&c);
+}
+#endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+
+#endif // if 0
+
+double
+static get_time(void)
+{
+ struct timeval curr;
+ gettimeofday(&curr,NULL);
+ return(curr.tv_sec + curr.tv_usec / 1e6);
+}
Added: pkg/trunk/nav/wavefront/src/plan.h
===================================================================
--- pkg/trunk/nav/wavefront/src/plan.h (rev 0)
+++ pkg/trunk/nav/wavefront/src/plan.h 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,233 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Andrew Howard
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**************************************************************************
+ * Desc: Path planning
+ * Author: Andrew Howard
+ * Date: 10 Oct 2002
+ * CVS: $Id: plan.h 6400 2008-05-08 02:01:26Z gerkey $
+**************************************************************************/
+
+#ifndef PLAN_H
+#define PLAN_H
+
+#include "heap.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define PLAN_DEFAULT_HEAP_SIZE 1000
+#define PLAN_MAX_COST 1e9
+
+// Description for a grid single cell
+typedef struct _plan_cell_t
+{
+ // Cell index in grid map
+ unsigned short ci, cj;
+
+ // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
+ char occ_state;
+ char occ_state_dyn;
+
+ // Distance to the nearest occupied cell
+ float occ_dist;
+ float occ_dist_dyn;
+
+ // Distance (cost) to the goal
+ float plan_cost;
+
+ // Mark used in dynamic programming
+ char mark;
+ // Mark used in path hysterisis
+ char lpathmark;
+
+ // The next cell in the plan
+ struct _plan_cell_t *plan_next;
+
+} plan_cell_t;
+
+
+// Planner info
+typedef struct
+{
+ // Grid dimensions (number of cells)
+ int size_x, size_y;
+
+ // Grid bounds (for limiting the search).
+ int min_x, min_y, max_x, max_y;
+
+ // Grid origin (real-world coords, in meters, of the lower-left grid
+ // cell)
+ double origin_x, origin_y;
+
+ // Grid scale (m/cell)
+ double scale;
+
+ // Effective robot radius
+ double des_min_radius, abs_min_radius;
+
+ // Max radius we will consider
+ double max_radius;
+
+ // Penalty factor for cells inside the max radius
+ double dist_penalty;
+
+ // Cost multiplier for cells on the previous local path
+ double hysteresis_factor;
+
+ // The grid data
+ plan_cell_t *cells;
+
+ // Distance penalty kernel, pre-computed in plan_compute_dist_kernel();
+ float* dist_kernel;
+ int dist_kernel_width;
+ float dist_kernel_3x3[9];
+
+ // Priority queue of cells to update
+ heap_t* heap;
+
+ // The global path
+ int path_count, path_size;
+ plan_cell_t **path;
+
+ // The local path (mainly for debugging)
+ int lpath_count, lpath_size;
+ plan_cell_t **lpath;
+
+ // Waypoints extracted from global path
+ int waypoint_count, waypoint_size;
+ plan_cell_t **waypoints;
+} plan_t;
+
+
+// Create a planner
+plan_t *plan_alloc(double abs_min_radius,
+ double des_min_radius,
+ double max_radius,
+ double dist_penalty,
+ double hysteresis_factor);
+
+void plan_compute_dist_kernel(plan_t* plan);
+
+// Destroy a planner
+void plan_free(plan_t *plan);
+
+// Initialize the plan
+void plan_init(plan_t *plan);
+
+// Reset the plan
+void plan_reset(plan_t *plan);
+
+#if 0
+// Load the occupancy values from an image file
+int plan_load_occ(plan_t *plan, const char *filename, double scale);
+#endif
+
+void plan_set_bounds(plan_t* plan, int min_x, int min_y, int max_x, int max_y);
+
+void plan_set_bbox(plan_t* plan, double padding, double min_size,
+ double x0, double y0, double x1, double y1);
+
+int plan_check_inbounds(plan_t* plan, double x, double y);
+
+// Construct the configuration space from the occupancy grid.
+//void plan_update_cspace(plan_t *plan, const char* cachefile);
+void plan_compute_cspace(plan_t *plan);
+
+int plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy);
+
+int plan_do_local(plan_t *plan, double lx, double ly, double plan_halfwidth);
+
+// Generate a path to the goal
+void plan_update_waypoints(plan_t *plan, double px, double py);
+
+// Get the ith waypoint; returns zero if there are no more waypoints
+int plan_get_waypoint(plan_t *plan, int i, double *px, double *py);
+
+// Convert given waypoint cell to global x,y
+void plan_convert_waypoint(plan_t* plan, plan_cell_t *waypoint,
+ double *px, double *py);
+
+double plan_get_carrot(plan_t* plan, double* px, double* py,
+ double lx, double ly,
+ double maxdist, double distweight);
+int plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
+ int* rotate_dir,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a,
+ double maxd, double dweight,
+ double tvmin, double tvmax,
+ double avmin, double avmax,
+ double amin, double amax);
+int plan_check_done(plan_t* plan,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a);
+
+void plan_set_obstacles(plan_t* plan, double* obs, size_t num);
+
+#if HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+// Write the cspace occupancy distance values to a file, one per line.
+// Read them back in with plan_read_cspace().
+// Returns non-zero on error.
+int plan_write_cspace(plan_t *plan, const char* fname, unsigned int* hash);
+
+// Read the cspace occupancy distance values from a file, one per line.
+// Write them in first with plan_read_cspace().
+// Returns non-zero on error.
+int plan_read_cspace(plan_t *plan, const char* fname, unsigned int* hash);
+
+// Compute and return the 16-bit MD5 hash of the map data in the given plan
+// object.
+void plan_md5(unsigned int* digest, plan_t* plan);
+#endif // HAVE_OPENSSL_MD5_H && HAVE_LIBCRYPTO
+
+/**************************************************************************
+ * Plan manipulation macros
+ **************************************************************************/
+
+// Convert from plan index to world coords
+//#define PLAN_WXGX(plan, i) (((i) - plan->size_x / 2) * plan->scale)
+//#define PLAN_WYGY(plan, j) (((j) - plan->size_y / 2) * plan->scale)
+#define PLAN_WXGX(plan, i) ((plan)->origin_x + (i) * (plan)->scale)
+#define PLAN_WYGY(plan, j) ((plan)->origin_y + (j) * (plan)->scale)
+
+// Convert from world coords to plan coords
+//#define PLAN_GXWX(plan, x) (floor((x) / plan->scale + 0.5) + plan->size_x / 2)
+//#define PLAN_GYWY(plan, y) (floor((y) / plan->scale + 0.5) + plan->size_y / 2)
+#define PLAN_GXWX(plan, x) ((int)(((x) - (plan)->origin_x) / (plan)->scale + 0.5))
+#define PLAN_GYWY(plan, y) ((int)(((y) - (plan)->origin_y) / (plan)->scale + 0.5))
+
+// Test to see if the given plan coords lie within the absolute plan bounds.
+#define PLAN_VALID(plan, i, j) ((i >= 0) && (i < plan->size_x) && (j >= 0) && (j < plan->size_y))
+// Test to see if the given plan coords lie within the user-specified plan bounds
+#define PLAN_VALID_BOUNDS(plan, i, j) ((i >= plan->min_x) && (i <= plan->max_x) && (j >= plan->min_y) && (j <= plan->max_y))
+
+// Compute the cell index for the given plan coords.
+#define PLAN_INDEX(plan, i, j) ((i) + (j) * plan->size_x)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Added: pkg/trunk/nav/wavefront/src/plan_control.c
===================================================================
--- pkg/trunk/nav/wavefront/src/plan_control.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/plan_control.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,297 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Brian Gerkey ge...@wi...
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <assert.h>
+
+#include "plan.h"
+
+static double _plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g);
+static double _angle_diff(double a, double b);
+
+int
+plan_check_done(plan_t* plan,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a)
+{
+ double dt, da;
+ dt = sqrt((gx-lx)*(gx-lx) + (gy-ly)*(gy-ly));
+ da = fabs(_angle_diff(ga,la));
+
+ if((dt < goal_d) && (da < goal_a))
+ return(1);
+ else
+ return(0);
+}
+
+int
+plan_compute_diffdrive_cmds(plan_t* plan, double* vx, double *va,
+ int* rotate_dir,
+ double lx, double ly, double la,
+ double gx, double gy, double ga,
+ double goal_d, double goal_a,
+ double maxd, double dweight,
+ double tvmin, double tvmax,
+ double avmin, double avmax,
+ double amin, double amax)
+{
+ double cx, cy;
+ double d,b,a,ad;
+
+ //puts("*******plan_compute_diffdrive_cmds************");
+
+ // Are we at the goal?
+ if(plan_check_done(plan,lx,ly,la,gx,gy,ga,goal_d,goal_a))
+ {
+ *vx = 0.0;
+ *va = 0.0;
+ return(0);
+ }
+
+ // Are we on top of the goal?
+ d = sqrt((gx-lx)*(gx-lx)+(gy-ly)*(gy-ly));
+ //printf("d: %.3f\n", d);
+ if(d < goal_d)
+ {
+ ad = _angle_diff(ga,la);
+ if(!*rotate_dir)
+ {
+ if(ad < 0)
+ *rotate_dir = -1;
+ else
+ *rotate_dir = 1;
+ }
+ *vx = 0.0;
+ *va = *rotate_dir * (avmin + (fabs(ad)/M_PI) * (avmax-avmin));
+ //printf("on top; vx:%.3f va: %.3f\n", *vx, *va);
+ return(0);
+ }
+
+ *rotate_dir = 0;
+
+ // We're away from the goal; compute velocities
+ if(plan_get_carrot(plan, &cx, &cy, lx, ly, maxd, dweight) < 0.0)
+ {
+ //puts("no carrot");
+ return(-1);
+ }
+
+ d = sqrt((lx-cx)*(lx-cx) + (ly-cy)*(ly-cy));
+ b = atan2(cy - ly, cx - lx);
+ a = amin + (d / maxd) * (amax-amin);
+ //printf("a: %.3f\n", a*180.0/M_PI);
+ ad = _angle_diff(b,la);
+ //printf("ad: %.3f\n", ad*180.0/M_PI);
+
+ if(fabs(ad) > a)
+ *vx = 0.0;
+ else
+ *vx = tvmin + (d / maxd) * (tvmax-tvmin);
+ *va = avmin + (fabs(ad)/M_PI) * (avmax-avmin);
+ if(ad < 0)
+ *va = -*va;
+
+ //printf("away; vx:%.3f va: %.3f\n", *vx, *va);
+ return(0);
+}
+
+double
+plan_get_carrot(plan_t* plan, double* px, double* py,
+ double lx, double ly, double maxdist, double distweight)
+{
+ plan_cell_t* cell, *ncell;
+ int li, lj;
+ double dist, d;
+ double cost, bestcost;
+ char old_occ_state;
+ float old_occ_dist;
+
+ li = PLAN_GXWX(plan, lx);
+ lj = PLAN_GYWY(plan, ly);
+
+ cell = plan->cells + PLAN_INDEX(plan,li,lj);
+
+ // Latch and clear the obstacle state for the cell I'm in
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ old_occ_state = cell->occ_state_dyn;
+ old_occ_dist = cell->occ_dist_dyn;
+ cell->occ_state_dyn = -1;
+ cell->occ_dist_dyn = plan->max_radius;
+
+ // Step back from maxdist, looking for the best carrot
+ bestcost = -1.0;
+ for(dist = maxdist; dist >= plan->scale; dist -= plan->scale)
+ {
+ // Find a point the required distance ahead, following the cost gradient
+ d=plan->scale;
+ for(ncell = cell;
+ (ncell->plan_next && (d < dist));
+ ncell = ncell->plan_next, d+=plan->scale);
+
+ // Check whether the straight-line path is clear
+ if((cost = _plan_check_path(plan, cell, ncell)) < 0.0)
+ {
+ //printf("no path from (%d,%d) to (%d,%d)\n",
+ //cell->ci, cell->cj, ncell->ci, ncell->cj);
+ continue;
+ }
+
+ // Weight distance
+ cost += distweight * (1.0/(dist*dist));
+ if((bestcost < 0.0) || (cost < bestcost))
+ {
+ bestcost = cost;
+ *px = PLAN_WXGX(plan,ncell->ci);
+ *py = PLAN_WYGY(plan,ncell->cj);
+ }
+ }
+
+ // Restore the obstacle state for the cell I'm in
+ cell = plan->cells + PLAN_INDEX(plan, li, lj);
+ cell->occ_state_dyn = old_occ_state;
+ cell->occ_dist_dyn = old_occ_dist;
+
+ return(bestcost);
+}
+
+static double
+_plan_check_path(plan_t* plan, plan_cell_t* s, plan_cell_t* g)
+{
+ // Bresenham raytracing
+ int x0,x1,y0,y1;
+ int x,y;
+ int xstep, ystep;
+ char steep;
+ int tmp;
+ int deltax, deltay, error, deltaerr;
+ int obscost=0;
+
+ x0 = s->ci;
+ y0 = s->cj;
+
+ x1 = g->ci;
+ y1 = g->cj;
+
+ if(abs(y1-y0) > abs(x1-x0))
+ steep = 1;
+ else
+ steep = 0;
+
+ if(steep)
+ {
+ tmp = x0;
+ x0 = y0;
+ y0 = tmp;
+
+ tmp = x1;
+ x1 = y1;
+ y1 = tmp;
+ }
+
+ deltax = abs(x1-x0);
+ deltay = abs(y1-y0);
+ error = 0;
+ deltaerr = deltay;
+
+ x = x0;
+ y = y0;
+
+ if(x0 < x1)
+ xstep = 1;
+ else
+ xstep = -1;
+ if(y0 < y1)
+ ystep = 1;
+ else
+ ystep = -1;
+
+ if(steep)
+ {
+ if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->max_radius)
+ obscost += plan->dist_penalty *
+ (plan->max_radius -
+ plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn);
+ }
+ else
+ {
+ if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->max_radius)
+ obscost += plan->dist_penalty *
+ (plan->max_radius -
+ plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
+ }
+
+ while(x != (x1 + xstep * 1))
+ {
+ x += xstep;
+ error += deltaerr;
+ if(2*error >= deltax)
+ {
+ y += ystep;
+ error -= deltax;
+ }
+
+ if(steep)
+ {
+ if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else if(plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn < plan->max_radius)
+ obscost += plan->dist_penalty *
+ (plan->max_radius -
+ plan->cells[PLAN_INDEX(plan,y,x)].occ_dist_dyn);
+ }
+ else
+ {
+ if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->abs_min_radius)
+ return -1;
+ else if(plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn < plan->max_radius)
+ obscost += plan->dist_penalty *
+ (plan->max_radius -
+ plan->cells[PLAN_INDEX(plan,x,y)].occ_dist_dyn);
+ }
+ }
+
+ return(obscost);
+}
+
+#define ANG_NORM(a) atan2(sin((a)),cos((a)))
+static double
+_angle_diff(double a, double b)
+{
+ double d1, d2;
+ a = ANG_NORM(a);
+ b = ANG_NORM(b);
+ d1 = a-b;
+ d2 = 2*M_PI - fabs(d1);
+ if(d1 > 0)
+ d2 *= -1.0;
+ if(fabs(d1) < fabs(d2))
+ return(d1);
+ else
+ return(d2);
+}
+
Added: pkg/trunk/nav/wavefront/src/plan_plan.c
===================================================================
--- pkg/trunk/nav/wavefront/src/plan_plan.c (rev 0)
+++ pkg/trunk/nav/wavefront/src/plan_plan.c 2009-04-21 04:29:39 UTC (rev 14130)
@@ -0,0 +1,388 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2008-
+ * Andrew Howard
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/**************************************************************************
+ * Desc: Path planner: plan generation
+ * Author: Andrew Howard
+ * Date: 10 Oct 2002
+ * CVS: $Id: plan_plan.c 6833 2008-07-10 01:34:50Z gerkey $
+**************************************************************************/
+
+#include <assert.h>
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <limits.h>
+#include <float.h>
+
+#include <sys/time.h>
+static double get_time(void);
+
+#include "plan.h"
+
+// Plan queue stuff
+void plan_push(plan_t *plan, plan_cell_t *cell);
+plan_cell_t *plan_pop(plan_t *plan);
+int _plan_update_plan(plan_t *plan, double lx, double ly, double gx, double gy);
+int _plan_find_local_goal(plan_t *plan, double* gx, double* gy, double lx, double ly);
+
+
+int
+plan_do_global(plan_t *plan, double lx, double ly, double gx, double gy)
+{
+ plan_cell_t* cell;
+ int li, lj;
+ double t0,t1;
+
+ t0 = get_time();
+
+ // Set bounds to look over the entire grid
+ plan_set_bounds(plan, 0, 0, plan->size_x - 1, plan->size_y - 1);
+
+ // Reset plan costs
+ plan_reset(plan);
+
+ plan->path_count = 0;
+ if(_plan_update_plan(plan, lx, ly, gx, gy) < 0)
+ {
+ // no path
+ return(-1);
+ }
+
+ li = PLAN_GXWX(plan, lx);
+ lj = PLAN_GYWY(plan, ly);
+
+ // Cache the path
+ for(cell = plan->cells + PLAN_INDEX(plan,li,lj);
+ cell;
+ cell = cell->plan_next)
+ {
+ if(plan->path_count >= plan->path_size)
+ {
+ plan->path_size *= 2;
+ plan->path = (plan_cell_t**)realloc(plan->path,
+ plan->path_size * sizeof(plan_cell_t*));
+ assert(plan->path);
+ }
+ plan->path[plan->path_count++] = cell;
+ }
+
+ t1 = get_time();
+
+ //printf("computed global path: %.6lf\n", t1-t0);
+
+ return(0);
+}
+
+int
+plan_do_local(plan_t *plan, double lx, double ly, doub...
[truncated message content] |
