Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14879] pkg/trunk
|
From: <jfa...@us...> - 2009-05-05 06:10:44
|
Revision: 14879
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14879&view=rev
Author: jfaustwg
Date: 2009-05-05 06:10:34 +0000 (Tue, 05 May 2009)
Log Message:
-----------
#if'd fixes against ros-trunk
Modified Paths:
--------------
pkg/trunk/nav/map_server/src/main.cpp
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
Modified: pkg/trunk/nav/map_server/src/main.cpp
===================================================================
--- pkg/trunk/nav/map_server/src/main.cpp 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/nav/map_server/src/main.cpp 2009-05-05 06:10:34 UTC (rev 14879)
@@ -118,7 +118,11 @@
*/
robot_srvs::StaticMap::Response map_resp_;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ void metadataSubscriptionCallback(const ros::SingleSubscriberPublisher& pub)
+#else
void metadataSubscriptionCallback(const ros::PublisherPtr& pub)
+#endif
{
publish( "map_metadata", meta_data_message_ );
}
@@ -164,7 +168,7 @@
{
fprintf(stderr, "%s\n", e.what());
}
-
+
return 0;
}
Modified: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-05-05 06:10:34 UTC (rev 14879)
@@ -36,7 +36,12 @@
publish("goal", wf_goal);
ros::Duration().fromSec(0.5).sleep(); // hack to try and wait for the message to go
}
+
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ void goal_subscriber_callback(const ros::SingleSubscriberPublisher& pub)
+#else
void goal_subscriber_callback(const ros::PublisherPtr& pub)
+#endif
{
publish("goal", wf_goal);
}
@@ -66,6 +71,6 @@
}
else
printf("success\n");
-
+
return 0;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-05-05 03:54:44 UTC (rev 14878)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-05-05 06:10:34 UTC (rev 14879)
@@ -30,6 +30,8 @@
#include <pr2_mechanism_controllers/TrajectoryWait.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include "ros/node.h"
+
class ROSRobotController : public ControllerBase
{
enum ControllerState {
@@ -51,20 +53,24 @@
{
assert(pnode != NULL);
Destroy();
-
+
_srvTrajectoryStart = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryStart::Request, pr2_mechanism_controllers::TrajectoryStart::Response>(_strTrajectoryServiceDir+"TrajectoryStart", true);
_srvTrajectoryCancel = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryCancel::Request, pr2_mechanism_controllers::TrajectoryCancel::Response>(_strTrajectoryServiceDir+"TrajectoryCancel", true);
_srvTrajectoryWait = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryWait::Request, pr2_mechanism_controllers::TrajectoryWait::Response>(_strTrajectoryServiceDir+"TrajectoryWait", true);
_srvTrajectoryQuery = ros::service::createHandle<pr2_mechanism_controllers::TrajectoryQuery::Request, pr2_mechanism_controllers::TrajectoryQuery::Response>(_strTrajectoryServiceDir+"TrajectoryQuery", true);
-
+
if( !_srvTrajectoryQuery ) {
RAVELOG_ERRORA("failed to find %s service\n", (_strTrajectoryServiceDir+"TrajectoryQuery").c_str());
return false;
}
-
+
pr2_mechanism_controllers::TrajectoryQuery::Request req;
pr2_mechanism_controllers::TrajectoryQuery::Response res;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( !_srvTrajectoryQuery.call(req,res) ) {
+#else
if( !_srvTrajectoryQuery->call(req,res) ) {
+#endif
RAVELOG_ERRORA("failed to query trajectory service %s\n", _strTrajectoryServiceDir.c_str());
return false;
}
@@ -83,7 +89,7 @@
_vjointmap.reserve(res.jointnames.size());
FOREACH(itname, res.jointnames) {
vector<string>::iterator itindex = find(vrobotjoints.begin(), vrobotjoints.end(), *itname);
- if( itindex == vrobotjoints.end() ) {
+ if( itindex == vrobotjoints.end() ) {
RAVELOG_ERRORA("failed to find joint %s\n", itname->c_str());
Destroy();
return false;
@@ -102,10 +108,18 @@
{
_vjointmap.clear();
_listTrajectories.clear();
+
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ _srvTrajectoryStart = ros::ServiceClient();
+ _srvTrajectoryCancel = ros::ServiceClient();
+ _srvTrajectoryWait = ros::ServiceClient();
+ _srvTrajectoryQuery = ros::ServiceClient();
+#else
_srvTrajectoryStart.reset();
_srvTrajectoryCancel.reset();
_srvTrajectoryWait.reset();
_srvTrajectoryQuery.reset();
+#endif
return true;
}
@@ -120,7 +134,11 @@
// trajectory services
RobotBase* _probot;
string _strTrajectoryServiceDir;
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ ros::ServiceClient _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+#else
service::ServiceHandlePtr _srvTrajectoryStart, _srvTrajectoryCancel, _srvTrajectoryWait, _srvTrajectoryQuery;
+#endif
vector<int> _vjointmap;
list<uint32_t> _listTrajectories; ///< trajectories currently pending for completion
};
@@ -235,9 +253,9 @@
bool bSuccess = true;
pr2_mechanism_controllers::TrajectoryStart::Request req;
pr2_mechanism_controllers::TrajectoryStart::Response res;
-
+
vector<dReal> vnewvalues;
-
+
{
RobotBase::RobotStateSaver saver(_probot);
_probot->SetJointValues(NULL, NULL, pValues,true);
@@ -276,7 +294,11 @@
(*ittrajcontroller)->GetTrajPoint(_vcurvalues, req.traj.points[0]);
(*ittrajcontroller)->GetTrajPoint(vnewvalues, req.traj.points[1]);
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( (*ittrajcontroller)->_srvTrajectoryStart.call(req,res) )
+#else
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+#endif
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
else {
RAVELOG_ERRORA("failed to start trajectory\n");
@@ -301,7 +323,7 @@
bool bSuccess = true;
pr2_mechanism_controllers::TrajectoryStart::Request req;
pr2_mechanism_controllers::TrajectoryStart::Response res;
-
+
FOREACH(ittrajcontroller, _listControllers) {
if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
RAVELOG_ERRORA("no start trajectory service\n");
@@ -313,11 +335,15 @@
_bIsDone = false;
req.hastiming = 0;
req.traj.points.resize(ptraj->GetPoints().size());
- typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
+ typeof(req.traj.points.begin()) ittraj = req.traj.points.begin();
FOREACHC(itpoint, ptraj->GetPoints())
(*ittrajcontroller)->GetTrajPoint(itpoint->q, *ittraj++);
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( (*ittrajcontroller)->_srvTrajectoryStart.call(req,res) )
+#else
if( (*ittrajcontroller)->_srvTrajectoryStart->call(req,res) )
+#endif
(*ittrajcontroller)->_listTrajectories.push_back(res.trajectoryid);
else {
RAVELOG_ERRORA("failed to start trajectory\n");
@@ -415,7 +441,7 @@
FOREACHC(itj, _mapjoints)
vel[itj->second] = _mstate.joint_states[itj->first].velocity;
}
-
+
virtual void GetTorque(std::vector<dReal>& torque) const
{
torque.resize(0);
@@ -485,7 +511,7 @@
break;
}
}
-
+
if( !bAdded ) {
RAVELOG_WARNA("could not find robot joint %s in mechanism state\n", itj->first.c_str());
break;
@@ -510,7 +536,7 @@
_mstate = _mstate_cb;
}
-
+
// do some monitoring of the joint state (try to look for stalls)
}
@@ -523,7 +549,7 @@
// check if the first trajectory is done
boost::mutex::scoped_lock lock(_mutexTrajectories);
-
+
if( _listControllers.size() > 0 ) {
bool bPopTrajectory = true;
@@ -535,11 +561,15 @@
break;
}
req.trajectoryid = (*ittraj)->_listTrajectories.front();
+#if ROS_VERSION_MINIMUM(0, 5, 0)
+ if( !(*ittraj)->_srvTrajectoryQuery.call(req,res) ) {
+#else
if( !(*ittraj)->_srvTrajectoryQuery->call(req,res) ) {
+#endif
RAVELOG_ERRORA("trajectory query failed\n");
bPopTrajectory = false;
}
-
+
if( !(res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Done ||
res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Deleted ||
res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Failed ||
@@ -568,7 +598,7 @@
RobotBase* _probot; ///< robot owning this controller
string _topic;
-
+
robot_msgs::MechanismState _mstate_cb, _mstate;
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
@@ -577,7 +607,7 @@
ofstream flog;
int logid;
-
+
map<int, int> _mapjoints; ///< maps mechanism state joints to robot joints
list<boost::shared_ptr<TrajectoryController> > _listControllers;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
