Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[14612] pkg/trunk
|
From: <tf...@us...> - 2009-04-29 08:00:30
|
Revision: 14612
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=14612&view=rev
Author: tfoote
Date: 2009-04-29 08:00:27 +0000 (Wed, 29 Apr 2009)
Log Message:
-----------
moving pr2_robot_actions/MoveBaseStateNew to nav_robot_actions/MoveBaseState and deleting the deprecated version. Porting all usages to the new version. And moving all usages of the new one to the new location.
Modified Paths:
--------------
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/highlevel/doors/doors_core/manifest.xml
pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
pkg/trunk/highlevel/nav/include/nav/move_base.h
pkg/trunk/highlevel/nav/include/nav/move_base_local.h
pkg/trunk/highlevel/nav/manifest.xml
pkg/trunk/highlevel/nav/src/move_base.cpp
pkg/trunk/highlevel/nav/src/move_base_local.cpp
pkg/trunk/nav/wavefront/manifest.xml
pkg/trunk/nav/wavefront/src/cli.cpp
pkg/trunk/nav/wavefront/src/wavefront_node.cpp
Added Paths:
-----------
pkg/trunk/nav/nav_robot_actions/CMakeLists.txt
pkg/trunk/nav/nav_robot_actions/Makefile
pkg/trunk/nav/nav_robot_actions/msg/
pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg
Removed Paths:
-------------
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-04-29 08:00:27 UTC (rev 14612)
@@ -48,7 +48,7 @@
import rospy, rostest
from std_msgs.msg import *
from robot_actions.msg import *
-from pr2_robot_actions.msg import *
+from nav_robot_actions.msg import *
from robot_msgs.msg import *
from deprecated_msgs.msg import *
from tf.transformations import *
Modified: pkg/trunk/highlevel/doors/doors_core/manifest.xml
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/doors/doors_core/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -13,6 +13,7 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
<depend package="robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="kdl" />
<depend package="angles" />
Modified: pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp
===================================================================
--- pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/doors/doors_core/test/test_executive.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -46,7 +46,7 @@
#include <pr2_robot_actions/DoorActionState.h>
#include <robot_actions/NoArgumentsActionState.h>
#include <pr2_robot_actions/SwitchControllersState.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include "doors_core/executive_functions.h"
@@ -92,7 +92,7 @@
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> open_door("open_door");
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> release_handle("release_handle");
robot_actions::ActionClient<robot_msgs::Door, pr2_robot_actions::DoorActionState, robot_msgs::Door> move_base_door("move_base_door");
- robot_actions::ActionClient<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped> move_base_local("move_base_local");
+ robot_actions::ActionClient<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped> move_base_local("move_base_local");
timeout_medium.sleep();
robot_msgs::Door tmp_door;
Modified: pkg/trunk/highlevel/executive_python/src/navigation_adapter.py
===================================================================
--- pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_python/src/navigation_adapter.py 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,14 +38,14 @@
roslib.load_manifest('executive_python')
import rospy
import random
-from pr2_robot_actions.msg import MoveBaseStateNew
+from nav_robot_actions.msg import MoveBaseState
from robot_msgs.msg import PoseStamped
class NavigationAdapter:
def __init__(self, no_plan_limit, time_limit, state_topic, goal_topic):
self.no_plan_limit = no_plan_limit
self.time_limit = time_limit
- rospy.Subscriber(state_topic, MoveBaseStateNew, self.update)
+ rospy.Subscriber(state_topic, MoveBaseState, self.update)
self.pub = rospy.Publisher(goal_topic, PoseStamped)
self.state = None
self.start_time = rospy.get_time()
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/include/executive_trex_pr2/adapters.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -53,7 +53,7 @@
#include <pr2_robot_actions/CheckPathState.h>
#include <pr2_robot_actions/NotifyDoorBlockedState.h>
#include <pr2_robot_actions/ShellCommandState.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <pr2_robot_actions/RechargeState.h>
#include <pr2_robot_actions/DetectPlugOnBaseState.h>
#include <pr2_robot_actions/MoveAndGraspPlugState.h>
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -10,6 +10,7 @@
<depend package="doors_core" />
<depend package="outlet_detection" />
<depend package="robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="pr2_robot_actions" />
<depend package="pr2_msgs" />
<depend package="std_msgs" />
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/adapters.cc 2009-04-29 08:00:27 UTC (rev 14612)
@@ -74,11 +74,11 @@
/***********************************************************************
* @brief MoveBase actions with a pose message for goal and feedback
**********************************************************************/
- class MoveBaseAdapter: public ROSActionAdapter<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped> {
+ class MoveBaseAdapter: public ROSActionAdapter<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped> {
public:
MoveBaseAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSActionAdapter<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(agentName, configData){
+ : ROSActionAdapter<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(agentName, configData){
}
virtual void fillDispatchParameters(robot_msgs::PoseStamped& msg, const TokenId& goalToken){
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/stub_ros_container.cc 2009-04-29 08:00:27 UTC (rev 14612)
@@ -247,11 +247,11 @@
// Navigation actions
executive_trex_pr2::SimpleStubAction<robot_msgs::PoseStamped> move_base("move_base");
if (getComponentParam("/trex/enable_move_base"))
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
executive_trex_pr2::SimpleStubAction<robot_msgs::PoseStamped> move_base_local("move_base_local");
if (getComponentParam("/trex/enable_move_base_local"))
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base_local);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base_local);
// Misc.
executive_trex_pr2::SimpleStubAction<std_msgs::Float32> recharge("recharge_controller");
Modified: pkg/trunk/highlevel/nav/include/nav/move_base.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/include/nav/move_base.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,7 +38,7 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/highlevel/nav/include/nav/move_base_local.h
===================================================================
--- pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/include/nav/move_base_local.h 2009-04-29 08:00:27 UTC (rev 14612)
@@ -38,7 +38,7 @@
#define NAV_MOVE_BASE_ACTION_H_
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
-#include <pr2_robot_actions/MoveBaseStateNew.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <ros/node.h>
#include <costmap_2d/costmap_2d_ros.h>
Modified: pkg/trunk/highlevel/nav/manifest.xml
===================================================================
--- pkg/trunk/highlevel/nav/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -14,6 +14,7 @@
<depend package="deprecated_msgs"/>
<depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
+ <depend package="nav_robot_actions"/>
<depend package="base_local_planner"/>
<depend package="costmap_2d"/>
<depend package="tf"/>
Modified: pkg/trunk/highlevel/nav/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/src/move_base.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -261,7 +261,7 @@
nav::MoveBase move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
ros_node.spin();
Modified: pkg/trunk/highlevel/nav/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -39,7 +39,7 @@
using namespace base_local_planner;
using namespace costmap_2d;
using namespace robot_actions;
-using namespace pr2_robot_actions;
+using namespace nav_robot_actions;
namespace nav {
MoveBaseLocal::MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf) :
@@ -215,7 +215,7 @@
nav::MoveBaseLocal move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
- runner.connect<robot_msgs::PoseStamped, pr2_robot_actions::MoveBaseStateNew, robot_msgs::PoseStamped>(move_base);
+ runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
ros_node.spin();
Deleted: pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg
===================================================================
--- pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseState.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -1,11 +0,0 @@
-# This will be deprecated
-Header header
-
-# Status
-robot_actions/ActionStatus status
-
-# Target position and orientation
-Pose2D goal
-
-# Actual position and orientation
-Pose2D feedback
\ No newline at end of file
Deleted: pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg
===================================================================
--- pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/highlevel/pr2_robot_actions/msg/MoveBaseStateNew.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -1,10 +0,0 @@
-Header header
-
-# Status
-robot_actions/ActionStatus status
-
-# Target position and orientation
-robot_msgs/PoseStamped goal
-
-# Actual position and orientation
-robot_msgs/PoseStamped feedback
Copied: pkg/trunk/nav/nav_robot_actions/CMakeLists.txt (from rev 14422, pkg/trunk/highlevel/pr2_robot_actions/CMakeLists.txt)
===================================================================
--- pkg/trunk/nav/nav_robot_actions/CMakeLists.txt (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/CMakeLists.txt 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(robot_actions)
+genmsg()
Copied: pkg/trunk/nav/nav_robot_actions/Makefile (from rev 14422, pkg/trunk/highlevel/pr2_robot_actions/Makefile)
===================================================================
--- pkg/trunk/nav/nav_robot_actions/Makefile (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/Makefile 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,2 @@
+include $(shell rospack find mk)/cmake.mk
+
Added: pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg
===================================================================
--- pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg (rev 0)
+++ pkg/trunk/nav/nav_robot_actions/msg/MoveBaseState.msg 2009-04-29 08:00:27 UTC (rev 14612)
@@ -0,0 +1,10 @@
+Header header
+
+# Status
+robot_actions/ActionStatus status
+
+# Target position and orientation
+robot_msgs/PoseStamped goal
+
+# Actual position and orientation
+robot_msgs/PoseStamped feedback
Modified: pkg/trunk/nav/wavefront/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront/manifest.xml 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/manifest.xml 2009-04-29 08:00:27 UTC (rev 14612)
@@ -8,6 +8,6 @@
<depend package="robot_srvs" />
<depend package="robot_msgs" />
<depend package="robot_actions" />
- <depend package="pr2_robot_actions" />
+ <depend package="nav_robot_actions" />
<depend package="tf"/>
</package>
Modified: pkg/trunk/nav/wavefront/src/cli.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/src/cli.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -2,14 +2,14 @@
#include <stdlib.h>
#include "ros/node.h"
#include "ros/publisher.h"
-#include "pr2_robot_actions/MoveBaseState.h"
+#include "nav_robot_actions/MoveBaseState.h"
#include "robot_msgs/PoseStamped.h"
#include "tf/tf.h"
class WavefrontCLI : public ros::Node
{
public:
- pr2_robot_actions::MoveBaseState wf_state;
+ nav_robot_actions::MoveBaseState wf_state;
robot_msgs::PoseStamped wf_goal;
enum { WF_IDLE, WF_SEEKING_GOAL, WF_DONE } state;
Modified: pkg/trunk/nav/wavefront/src/wavefront_node.cpp
===================================================================
--- pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-29 05:28:12 UTC (rev 14611)
+++ pkg/trunk/nav/wavefront/src/wavefront_node.cpp 2009-04-29 08:00:27 UTC (rev 14612)
@@ -67,7 +67,7 @@
Publishes to (name / type):
- @b "cmd_vel" robot_msgs/PoseDot : velocity commands to robot
-- @b "state" pr2_robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
+- @b "state" nav_robot_actions/MoveBaseState : current planner state (e.g., goal reached, no path)
- @b "gui_path" robot_msgs/Polyline : current global path (for visualization)
- @b "gui_laser" robot_msgs/Polyline : re-projected laser scans (for visualization)
@@ -101,7 +101,7 @@
#include "boost/thread/mutex.hpp"
// The messages that we'll use
-#include <pr2_robot_actions/MoveBaseState.h>
+#include <nav_robot_actions/MoveBaseState.h>
#include <robot_msgs/PoseStamped.h>
#include <robot_msgs/PoseDot.h>
#include <robot_msgs/PointCloud.h>
@@ -149,7 +149,7 @@
REACHED_GOAL
} planner_state;
- tf::Stamped<btTransform> global_pose;
+ tf::Stamped<btTransform> global_pose_;
// If we can't reach the goal, note when it happened and keep trying a bit
//ros::Time stuck_time;
@@ -157,6 +157,7 @@
bool enable;
// Current goal
double goal[3];
+ tf::Stamped<tf::Pose> goalPose_;
// Direction that we're rotating in order to assume the goal
// orientation
int rotate_dir;
@@ -197,7 +198,7 @@
//MsgRobotBase2DOdom odomMsg;
robot_msgs::Polyline polylineMsg;
robot_msgs::Polyline pointcloudMsg;
- pr2_robot_actions::MoveBaseState pstate;
+ nav_robot_actions::MoveBaseState pstate;
//MsgRobotBase2DOdom prevOdom;
bool firstodom;
@@ -283,7 +284,7 @@
tf(*this, true, ros::Duration(10)) // cache for 10 sec, no extrapolation
{
// Initialize global pose. Will be set in control loop based on actual data.
- ///\todo does this need to be initialized? global_pose.setIdentity();
+ ///\todo does this need to be initialized? global_pose_.setIdentity();
// set a few parameters. leave defaults just as in the ctor initializer list
param("~dist_eps", dist_eps, 1.0);
@@ -360,7 +361,7 @@
this->firstodom = true;
- advertise<pr2_robot_actions::MoveBaseState>("state",1);
+ advertise<nav_robot_actions::MoveBaseState>("state",1);
advertise<robot_msgs::Polyline>("gui_path",1);
advertise<robot_msgs::Polyline>("gui_laser",1);
advertise<robot_msgs::PoseDot>("cmd_vel",1);
@@ -384,15 +385,14 @@
this->enable = 1;
if(this->enable){
- tf::Stamped<tf::Pose> goalPose;
- tf::PoseStampedMsgToTF(goalMsg, goalPose);
+ tf::PoseStampedMsgToTF(goalMsg, this->goalPose_);
// Populate goal data
- this->goal[0] = goalPose.getOrigin().getX();
- this->goal[1] = goalPose.getOrigin().getY();
+ this->goal[0] = this->goalPose_.getOrigin().getX();
+ this->goal[1] = this->goalPose_.getOrigin().getY();
double yaw, pitch, roll;
- goalPose.getBasis().getEulerZYX(yaw, pitch, roll);
+ this->goalPose_.getBasis().getEulerZYX(yaw, pitch, roll);
this->goal[2] = yaw;
this->planner_state = NEW_GOAL;
@@ -412,17 +412,12 @@
this->pstate.status.value = this->pstate.status.SUCCESS;
}
- double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.getBasis();
- mat.getEulerZYX(yaw, pitch, roll);
+ robot_msgs::PoseStamped pose_msg;
+ tf::PoseStampedTFToMsg(global_pose_, pose_msg);
// Fill out and publish response
- this->pstate.feedback.x = global_pose.getOrigin().x();
- this->pstate.feedback.y = global_pose.getOrigin().y();
- this->pstate.feedback.th = yaw;
- this->pstate.goal.x = this->goal[0];
- this->pstate.goal.y = this->goal[1];
- this->pstate.goal.th = this->goal[2];
+ this->pstate.feedback = pose_msg;
+ this->pstate.goal = goalMsg;
publish("state",this->pstate);
this->lock.unlock();
}
@@ -579,7 +574,7 @@
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
- this->tf.transformPose("/map", robotPose, global_pose);
+ this->tf.transformPose("/map", robotPose, global_pose_);
}
catch(tf::LookupException& ex)
{
@@ -626,12 +621,12 @@
{
double yaw,pitch,roll; //fixme make cleaner namespace
- btMatrix3x3 mat = global_pose.getBasis();
+ btMatrix3x3 mat = global_pose_.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
// Are we done?
if(plan_check_done(this->plan,
- global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw,
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(), yaw,
this->goal[0], this->goal[1], this->goal[2],
this->dist_eps, this->ang_eps))
{
@@ -651,11 +646,11 @@
// Try a local plan
if((this->planner_state == NEW_GOAL) ||
- (plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ (plan_do_local(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->plan_halfwidth) < 0))
{
// Fallback on global plan
- if(plan_do_global(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ if(plan_do_global(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->goal[0], this->goal[1]) < 0)
{
// no global plan
@@ -685,7 +680,7 @@
{
// global plan succeeded; now try the local plan again
this->printed_warning = false;
- if(plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ if(plan_do_local(this->plan, global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
this->plan_halfwidth) < 0)
{
// no local plan; better luck next time through
@@ -706,12 +701,12 @@
// double yaw,pitch,roll; //used temporarily earlier fixme make cleaner
//btMatrix3x3
- mat = global_pose.getBasis();
+ mat = global_pose_.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
if(plan_compute_diffdrive_cmds(this->plan, &vx, &va,
&this->rotate_dir,
- global_pose.getOrigin().x(), global_pose.getOrigin().y(),
+ global_pose_.getOrigin().x(), global_pose_.getOrigin().y(),
yaw,
this->goal[0], this->goal[1],
this->goal[2],
@@ -759,21 +754,16 @@
default:
assert(0);
}
- double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.getBasis();
- mat.getEulerZYX(yaw, pitch, roll);
-
if(this->enable && (this->planner_state == PURSUING_GOAL))
this->pstate.status.value = this->pstate.status.ACTIVE;
else
this->pstate.status.value = this->pstate.status.SUCCESS;
- this->pstate.feedback.x = global_pose.getOrigin().x();
- this->pstate.feedback.y = global_pose.getOrigin().y();
- this->pstate.feedback.th = yaw;
- this->pstate.goal.x = this->goal[0];
- this->pstate.goal.y = this->goal[1];
- this->pstate.goal.th = this->goal[2];
+ robot_msgs::PoseStamped pose_out;
+ tf::PoseStampedTFToMsg(global_pose_, pose_out);
+ this->pstate.feedback = pose_out;
+ tf::PoseStampedTFToMsg(this->goalPose_, pose_out);
+ this->pstate.goal = pose_out;
publish("state",this->pstate);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
