Revision: 9140
http://sourceforge.net/p/playerstage/svn/9140
Author: jpgr87
Date: 2014-02-18 02:50:47 +0000 (Tue, 18 Feb 2014)
Log Message:
-----------
Planner interface extensions: start position command and waypoint path length
Modified Paths:
--------------
code/player/trunk/client_libs/libplayerc/dev_planner.c
code/player/trunk/client_libs/libplayerc/playerc.h
code/player/trunk/client_libs/libplayerc++/plannerproxy.cc
code/player/trunk/client_libs/libplayerc++/playerc++.h
code/player/trunk/cmake/internal/FindOS.cmake
code/player/trunk/libplayerinterface/interfaces/044_planner.def
Modified: code/player/trunk/client_libs/libplayerc/dev_planner.c
===================================================================
--- code/player/trunk/client_libs/libplayerc/dev_planner.c 2014-02-18
02:50:05 UTC (rev 9139)
+++ code/player/trunk/client_libs/libplayerc/dev_planner.c 2014-02-18
02:50:47 UTC (rev 9140)
@@ -139,6 +139,21 @@
&cmd, NULL);
}
+int
+playerc_planner_set_cmd_start(playerc_planner_t *device,
+ double sx, double sy, double sa)
+{
+ player_planner_cmd_t cmd;
+
+ cmd.goal.px = sx;
+ cmd.goal.py = sy;
+ cmd.goal.pa = sa;
+
+ return playerc_client_write(device->info.client, &device->info,
+ PLAYER_PLANNER_CMD_START,
+ &cmd, NULL);
+}
+
// Get the list of waypoints. The writes the result into the proxy
// rather than returning it to the caller.
int playerc_planner_get_waypoints(playerc_planner_t *device)
@@ -160,6 +175,7 @@
device->waypoints[i][1] = config->waypoints[i].py;
device->waypoints[i][2] = config->waypoints[i].pa;
}
+ device->waypoint_distance = config->waypoints_distance;
player_planner_waypoints_req_t_free(config);
return 0;
}
Modified: code/player/trunk/client_libs/libplayerc/playerc.h
===================================================================
--- code/player/trunk/client_libs/libplayerc/playerc.h 2014-02-18 02:50:05 UTC
(rev 9139)
+++ code/player/trunk/client_libs/libplayerc/playerc.h 2014-02-18 02:50:47 UTC
(rev 9140)
@@ -2733,6 +2733,10 @@
playerc_planner_get_waypoints() to fill this in. */
double (*waypoints)[3];
+ /** Straight-line distance along allwaypoints in the current plan. Call
+ playerc_planner_get_waypoints() to fill this in. */
+ double waypoint_distance;
+
} playerc_planner_t;
/** @brief Create a planner device proxy. */
@@ -2751,6 +2755,10 @@
PLAYERC_EXPORT int playerc_planner_set_cmd_pose(playerc_planner_t *device,
double gx, double gy, double ga);
+/** @brief Set the start pose (sx, sy, sa) */
+PLAYERC_EXPORT int playerc_planner_set_cmd_start(playerc_planner_t *device,
+ double sx, double sy, double sa);
+
/** @brief Get the list of waypoints.
Writes the result into the proxy rather than returning it to the
Modified: code/player/trunk/client_libs/libplayerc++/plannerproxy.cc
===================================================================
--- code/player/trunk/client_libs/libplayerc++/plannerproxy.cc 2014-02-18
02:50:05 UTC (rev 9139)
+++ code/player/trunk/client_libs/libplayerc++/plannerproxy.cc 2014-02-18
02:50:47 UTC (rev 9140)
@@ -102,6 +102,15 @@
}
void
+PlannerProxy::SetStartPose(double aSx, double aSy, double aSa)
+{
+ scoped_lock_t lock(mPc->mMutex);
+ if (0 != playerc_planner_set_cmd_start(mDevice, aSx, aSy, aSa))
+ throw PlayerError("PlannerProxy::SetStartPose()", "error setting start");
+ return;
+}
+
+void
PlannerProxy::SetGoalPose(double aGx, double aGy, double aGa)
{
scoped_lock_t lock(mPc->mMutex);
Modified: code/player/trunk/client_libs/libplayerc++/playerc++.h
===================================================================
--- code/player/trunk/client_libs/libplayerc++/playerc++.h 2014-02-18
02:50:05 UTC (rev 9139)
+++ code/player/trunk/client_libs/libplayerc++/playerc++.h 2014-02-18
02:50:47 UTC (rev 9140)
@@ -1738,6 +1738,9 @@
/// Set the goal pose (gx, gy, ga)
void SetGoalPose(double aGx, double aGy, double aGa);
+ /// Set the start pose (sx, sy, sa)
+ void SetStartPose(double aSx, double aSy, double aSa);
+
/// Get the list of waypoints. Writes the result into the proxy
/// rather than returning it to the caller.
void RequestWaypoints();
@@ -1752,6 +1755,10 @@
/// Have we arrived at the goal?
uint32_t GetPathDone() const { return GetVar(mDevice->path_done); };
+ /// Get straight-line distance along path. Call RequestWaypoints() to
+ /// fill in.
+ double GetPathLength() const {return GetVar(mDevice->waypoint_distance); };
+
/// @brief Current pose (m)
/// @deprecated use GetPose() instead
double GetPx() const { return GetVar(mDevice->px); };
Modified: code/player/trunk/cmake/internal/FindOS.cmake
===================================================================
--- code/player/trunk/cmake/internal/FindOS.cmake 2014-02-18 02:50:05 UTC
(rev 9139)
+++ code/player/trunk/cmake/internal/FindOS.cmake 2014-02-18 02:50:47 UTC
(rev 9140)
@@ -64,6 +64,3 @@
ENDIF (${CMAKE_SYSTEM_PROCESSOR} STREQUAL "x86_64")
ENDIF (PLAYER_OS_LINUX)
-
-
-##/RJM 10/09
Modified: code/player/trunk/libplayerinterface/interfaces/044_planner.def
===================================================================
--- code/player/trunk/libplayerinterface/interfaces/044_planner.def
2014-02-18 02:50:05 UTC (rev 9139)
+++ code/player/trunk/libplayerinterface/interfaces/044_planner.def
2014-02-18 02:50:47 UTC (rev 9140)
@@ -8,8 +8,10 @@
/** Data subtype: state */
message { DATA, STATE, 1, player_planner_data_t };
-/** Command subtype: state */
+/** Command subtype: set goal position */
message { CMD, GOAL, 1, player_planner_cmd_t };
+/** Command subtype: set start position */
+message { CMD, START, 2, player_planner_cmd_t };
/** Request subtype: get waypoints */
message { REQ, GET_WAYPOINTS, 1, player_planner_waypoints_req_t };
@@ -41,7 +43,7 @@
uint32_t waypoints_count;
} player_planner_data_t;
-/** @brief Command: state (@ref PLAYER_PLANNER_CMD_GOAL)
+/** @brief Command: start or goal position (@ref PLAYER_PLANNER_CMD_GOAL, @ref
PLAYER_PLANNER_CMD_START)
The @p planner interface accepts a new goal. */
typedef struct player_planner_cmd
@@ -57,6 +59,8 @@
*/
typedef struct player_planner_waypoints_req
{
+ /** Straight-line distance along all waypoints */
+ double waypoints_distance;
/** Number of waypoints to follow */
uint32_t waypoints_count;
/** The waypoints */
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
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
