Revision: 9139
http://sourceforge.net/p/playerstage/svn/9139
Author: jpgr87
Date: 2014-02-18 02:50:05 +0000 (Tue, 18 Feb 2014)
Log Message:
-----------
Applied patch #625: Offline path computations with wavefront
Modified Paths:
--------------
code/player/trunk/server/drivers/planner/wavefront/plan.c
code/player/trunk/server/drivers/planner/wavefront/plan.h
code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.c
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.c 2014-02-18
00:28:27 UTC (rev 9138)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.c 2014-02-18
02:50:05 UTC (rev 9139)
@@ -212,6 +212,51 @@
return;
}
+// Copy the planner
+plan_t *plan_copy(plan_t *plan)
+{
+ int i;
+ plan_t* ret_plan;
+
+ ret_plan = plan_alloc(plan->abs_min_radius,
+ plan->des_min_radius,
+ plan->max_radius,
+ plan->dist_penalty,
+ plan->hysteresis_factor);
+
+ assert(ret_plan);
+
+ // Fill in the map structure
+ // First, get the map info
+ ret_plan->scale = plan->scale;
+ ret_plan->size_x = plan->size_x;
+ ret_plan->size_y = plan->size_y;
+ ret_plan->origin_x = plan->origin_x;
+ ret_plan->origin_y = plan->origin_y;
+
+ // Now get the map data
+ // Allocate space for map cells
+ ret_plan->cells = (plan_cell_t*)realloc(ret_plan->cells,
+ (ret_plan->size_x *
+ ret_plan->size_y *
+ sizeof(plan_cell_t)));
+ assert(ret_plan->cells);
+
+ // Do initialization
+ plan_init(ret_plan);
+
+ // Copy the map data
+ for (i = 0; i < ret_plan->size_x * ret_plan->size_y; i++)
+ {
+ ret_plan->cells[i].occ_dist = plan->cells[i].occ_dist;
+ ret_plan->cells[i].occ_state = plan->cells[i].occ_state;
+ ret_plan->cells[i].occ_state_dyn = plan->cells[i].occ_state_dyn;
+ ret_plan->cells[i].occ_dist_dyn = plan->cells[i].occ_dist_dyn;
+ }
+
+ return ret_plan;
+}
+
// Initialize the plan
void plan_init(plan_t *plan)
{
Modified: code/player/trunk/server/drivers/planner/wavefront/plan.h
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/plan.h 2014-02-18
00:28:27 UTC (rev 9138)
+++ code/player/trunk/server/drivers/planner/wavefront/plan.h 2014-02-18
02:50:05 UTC (rev 9139)
@@ -133,6 +133,9 @@
// Destroy a planner
void plan_free(plan_t *plan);
+// Copy a planner
+plan_t *plan_copy(plan_t *plan);
+
// Initialize the plan
void plan_init(plan_t *plan);
Modified: code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
===================================================================
--- code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2014-02-18 00:28:27 UTC (rev 9138)
+++ code/player/trunk/server/drivers/planner/wavefront/wavefront.cc
2014-02-18 02:50:05 UTC (rev 9139)
@@ -197,7 +197,7 @@
driver
(
name "wavefront"
- provides ["planner:0"]
+ provides ["planner:0" "offline:::planner:1"]
requires ["output:::position2d:1" "input:::position2d:2" "map:0"]
safety_dist 0.15
distance_epsilon 0.5
@@ -209,12 +209,6 @@
*/
/** @} */
-// TODO:
-//
-// - allow for computing a path, without actually executing it.
-//
-// - compute and return path length
-
#include <string.h>
#include <stddef.h>
#include <stdlib.h>
@@ -249,6 +243,8 @@
player_devaddr_t map_id;
player_devaddr_t laser_id;
player_devaddr_t graphics2d_id;
+ player_devaddr_t offline_planner_id;
+
double map_res;
double robot_radius;
double safety_dist;
@@ -261,6 +257,8 @@
// the plan object
plan_t* plan;
+ // another plan object for offline path computation
+ plan_t* offline_plan;
// pointers to the underlying devices
Device* position;
@@ -325,7 +323,15 @@
double* scan_points;
int scan_points_size;
int scan_points_count;
+
+ // Do we have an offline planner
+ bool have_offline_planner;
+ // offline planner goal postion
+ player_pose2d_t offline_goal;
+ // offline planner start position
+ player_pose2d_t offline_start;
+
// methods for internal use
int SetupLocalize();
int SetupLaser();
@@ -352,6 +358,7 @@
void LocalizeToPosition(double* px, double* py, double* pa,
double lx, double ly, double la);
void SetWaypoint(double wx, double wy, double wa);
+ void ComputeOfflineWaypoints(player_planner_waypoints_req_t* req,
player_planner_waypoints_req_t* reply);
public:
// Constructor
@@ -385,9 +392,9 @@
////////////////////////////////////////////////////////////////////////////////
// Constructor
Wavefront::Wavefront( ConfigFile* cf, int section)
- : ThreadedDriver(cf, section, true,
- PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_PLANNER_CODE)
+ : ThreadedDriver(cf, section, true)
{
+ this->have_offline_planner = false;
// Must have a position device to control
if (cf->ReadDeviceAddr(&this->position_id, section, "requires",
PLAYER_POSITION2D_CODE, -1, "output") != 0)
@@ -410,6 +417,20 @@
return;
}
+ if (cf->ReadDeviceAddr(&this->device_addr, section, "provides",
+ PLAYER_PLANNER_CODE, -1, "") != 0)
+ {
+ PLAYER_ERROR("Wavefront must provide a Planner");
+ this->SetError(-1);
+ return;
+ }
+ if (cf->ReadDeviceAddr(&this->offline_planner_id, section, "provides",
+ PLAYER_PLANNER_CODE, -1, "offline") != 0)
+ {
+ this->have_offline_planner = true;
+ PLAYER_WARN("Wavefront providing offline planner");
+ }
+
// Can use a laser device
memset(&this->laser_id,0,sizeof(player_devaddr_t));
cf->ReadDeviceAddr(&this->laser_id, section, "requires",
@@ -460,6 +481,8 @@
if(this->velocity_control)
PLAYER_WARN("Wavefront doing direct velocity control, but without a
laser for obstacle detection; this is not safe!");
}
+ memset((void*)(&this->offline_goal), 0, sizeof(player_pose2d_t));
+ memset((void*)(&this->offline_start), 0, sizeof(player_pose2d_t));
}
@@ -500,7 +523,7 @@
PLAYER_ERROR("failed to allocate plan");
return(-1);
}
-
+ this->offline_plan = NULL;
if(SetupMap() < 0)
return(-1);
if(SetupLocalize() < 0)
@@ -539,6 +562,8 @@
if(this->plan)
plan_free(this->plan);
+ if(this->offline_plan)
+ plan_free(this->offline_plan);
free(this->waypoints);
this->waypoints = NULL;
@@ -579,6 +604,68 @@
}
void
+Wavefront::ComputeOfflineWaypoints(player_planner_waypoints_req_t* req,
player_planner_waypoints_req_t* reply)
+{
+ double sx, sy, sa, gx, gy, ga;
+
+ sx = this->offline_start.px;
+ sy = this->offline_start.py;
+ sa = this->offline_start.pa;
+
+ gx = this->offline_goal.px;
+ gy = this->offline_goal.py;
+ ga = this->offline_goal.pa;
+
+ // If there is no offline_plan, create by duplicating plan
+ if(!this->offline_plan)
+ this->offline_plan = plan_copy(this->plan);
+
+ // Compute path in offline plan
+ if(plan_do_global(this->offline_plan, sx, sy, gx, gy) < 0)
+ {
+ puts("Wavefront: offline path computation failed");
+ }
+
+ // Extract waypoints along the path to the goal from the start position
+ plan_update_waypoints(this->offline_plan, sx, sy);
+
+ // Fill in reply
+ // - waypoints
+ if((reply->waypoints_count = this->offline_plan->waypoint_count))
+ {
+ reply->waypoints =
(player_pose2d_t*)calloc(sizeof(reply->waypoints[0]),reply->waypoints_count);
+
+ double distance = 0;
+ double last_wx, last_wy;
+ for(int i=0;i<(int)reply->waypoints_count;i++)
+ {
+ // Convert and copy waypoint
+ double wx, wy;
+ plan_convert_waypoint(this->offline_plan,
+ this->offline_plan->waypoints[i],
+ &wx, &wy);
+ reply->waypoints[i].px = wx;
+ reply->waypoints[i].py = wy;
+ reply->waypoints[i].pa = 0.0;
+ // Update path length
+ if(i != 0)
+ {
+ distance += sqrt(pow(wx-last_wx, 2) + pow(wy-last_wy, 2));
+ }
+ last_wx = wx;
+ last_wy = wy;
+ }
+ reply->waypoints_distance = distance;
+ }
+ else // no waypoints
+ {
+ reply->waypoints = NULL;
+ reply->waypoints_distance = 0.0;
+ }
+}
+
+
+void
Wavefront::ProcessLaserScan(player_laser_data_scanpose_t* data)
{
double t0,t1;
@@ -1543,6 +1630,10 @@
plan_compute_cspace(this->plan);
//draw_cspace(this->plan,"cspace.png");
+ if (this->offline_plan) {
+ plan_free(this->offline_plan);
+ this->offline_plan = NULL;
+ }
return(0);
}
@@ -1659,7 +1750,13 @@
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_PLANNER_REQ_GET_WAYPOINTS);
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_PLANNER_REQ_ENABLE);
HANDLE_CAPABILITY_REQUEST (device_addr, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_PLANNER_CMD_GOAL);
-
+ if (this->have_offline_planner)
+ {
+ HANDLE_CAPABILITY_REQUEST (offline_planner_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_CAPABILITIES_REQ);
+ HANDLE_CAPABILITY_REQUEST (offline_planner_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_REQ, PLAYER_PLANNER_REQ_GET_WAYPOINTS);
+ HANDLE_CAPABILITY_REQUEST (offline_planner_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_PLANNER_CMD_GOAL);
+ HANDLE_CAPABILITY_REQUEST (offline_planner_id, resp_queue, hdr, data,
PLAYER_MSGTYPE_CMD, PLAYER_PLANNER_CMD_START);
+ }
// Is it new odometry data?
if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
@@ -1721,12 +1818,22 @@
reply.waypoints_count = this->waypoint_count;
reply.waypoints =
(player_pose2d_t*)calloc(sizeof(reply.waypoints[0]),this->waypoint_count);
+ double distance = 0;
+ double px, py, last_px, last_py;
for(int i=0;i<(int)reply.waypoints_count;i++)
{
- reply.waypoints[i].px = this->waypoints[i][0];
- reply.waypoints[i].py = this->waypoints[i][1];
+ // copy waypoint for length computation
+ px = reply.waypoints[i].px = this->waypoints[i][0];
+ py = reply.waypoints[i].py = this->waypoints[i][1];
reply.waypoints[i].pa = 0.0;
+ if(i != 0)
+ {
+ distance += sqrt(pow(px-last_px, 2) + pow(py-last_py, 2));
+ }
+ last_px = px;
+ last_py = py;
}
+ reply.waypoints_distance = distance;
this->Publish(this->device_addr, resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
@@ -1735,6 +1842,47 @@
free(reply.waypoints);
return(0);
}
+ // Is it a start position for an offline computed path?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_PLANNER_CMD_START,
+ this->offline_planner_id))
+ {
+ assert(data);
+ this->offline_start.px = ((player_planner_cmd_t*)data)->goal.px;
+ this->offline_start.py = ((player_planner_cmd_t*)data)->goal.py;
+ this->offline_start.pa = ((player_planner_cmd_t*)data)->goal.pa;
+ return(0);
+ }
+ // Is it a goal position for an offline computed path?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
+ PLAYER_PLANNER_CMD_GOAL,
+ this->offline_planner_id))
+ {
+ assert(data);
+ this->offline_goal.px = ((player_planner_cmd_t*)data)->goal.px;
+ this->offline_goal.py = ((player_planner_cmd_t*)data)->goal.py;
+ this->offline_goal.pa = ((player_planner_cmd_t*)data)->goal.pa;
+ return(0);
+ }
+
+ // Is it a request for an offline computed path?
+ else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
+ PLAYER_PLANNER_REQ_GET_WAYPOINTS,
+ this->offline_planner_id))
+ {
+ player_planner_waypoints_req_t reply;
+
+ assert(data);
+ this->ComputeOfflineWaypoints((player_planner_waypoints_req_t*)data,
&reply);
+
+ this->Publish(this->offline_planner_id, resp_queue,
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_PLANNER_REQ_GET_WAYPOINTS,
+ (void*)&reply);
+ if(reply.waypoints)
+ free(reply.waypoints);
+ return(0);
+ }
// Is it a request to enable or disable the planner?
else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_PLANNER_REQ_ENABLE,
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Managing the Performance of Cloud-Based Applications
Take advantage of what the Cloud has to offer - Avoid Common Pitfalls.
Read the Whitepaper.
http://pubads.g.doubleclick.net/gampad/clk?id=121054471&iu=/4140/ostg.clktrk
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
