Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21110] pkg/trunk
|
From: <jfa...@us...> - 2009-08-08 01:30:31
|
Revision: 21110
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21110&view=rev
Author: jfaustwg
Date: 2009-08-08 01:30:20 +0000 (Sat, 08 Aug 2009)
Log Message:
-----------
PoseWithCovariance->PoseWithCovarianceStamped
PoseWithCovarianceStamped::pose_with_covariance -> PoseWithCovarianceStamped::data
Modified Paths:
--------------
pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp
pkg/trunk/sandbox/webteleop/src/server.py
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg
pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h
pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp
pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp
pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/stacks/navigation/nav_view/src/nav_view/tools.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp
pkg/trunk/vision/vslam/nodes/picmap.py
pkg/trunk/vision/vslam/nodes/roadmap.py
Modified: pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-08 01:30:20 UTC (rev 21110)
@@ -49,7 +49,7 @@
from std_msgs.msg import String
from nav_robot_actions.msg import MoveBaseState
-from geometry_msgs.msg import Pose,Quaternion,Point, PoseWithRatesStamped, PoseStamped, PoseWithCovariance
+from geometry_msgs.msg import Pose,Quaternion,Point, PoseWithRatesStamped, PoseStamped, PoseWithCovarianceStamped
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
import tf.transformations as tft
@@ -222,13 +222,13 @@
#pub_base = rospy.Publisher("cmd_vel", BaseVel)
pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
- pub_pose = rospy.Publisher("initialpose", PoseWithCovariance)
+ pub_pose = rospy.Publisher("initialpose", PoseWithCovarianceStamped)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom" , Odometry , self.odomInput)
rospy.Subscriber("base_bumper/info" , String , self.bumpedInput)
rospy.Subscriber("torso_lift_bumper/info", String , self.bumpedInput)
rospy.Subscriber("/move_base/feedback" , MoveBaseState , self.stateInput)
- rospy.Subscriber("/amcl_pose" , PoseWithCovariance , self.amclInput)
+ rospy.Subscriber("/amcl_pose" , PoseWithCovarianceStamped , self.amclInput)
# below only for debugging build 303, base not moving
rospy.Subscriber("cmd_vel" , Twist , self.cmd_velInput)
@@ -312,7 +312,7 @@
q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
pose = Pose(p,q)
print "publishing initialpose",h,p,COV[0]
- pub_pose.publish(PoseWithCovariance(h, pose,COV))
+ pub_pose.publish(PoseWithCovarianceStamped(h, PoseWithCovariance(pose,COV)))
else:
# send goal until state /move_base/feedback indicates goal is received
p = Point(self.target_x, self.target_y, 0)
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-08 01:30:20 UTC (rev 21110)
@@ -48,7 +48,7 @@
import rospy, rostest
from std_msgs.msg import String
from nav_robot_actions.msg import MoveBaseState
-from geometry_msgs.msg import Pose,Quaternion,Point, PoseWithRatesStamped, PoseStamped, PoseWithCovariance
+from geometry_msgs.msg import Pose,Quaternion,Point, PoseWithRatesStamped, PoseStamped, PoseWithCovarianceStamped
from robot_msgs.msg import PoseDot
from nav_msgs.msg import Odometry
import tf.transformations as tft
@@ -223,13 +223,13 @@
#pub_base = rospy.Publisher("cmd_vel", BaseVel)
pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
- pub_pose = rospy.Publisher("initialpose", PoseWithCovariance)
+ pub_pose = rospy.Publisher("initialpose", PoseWithCovarianceStamped)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
rospy.Subscriber("odom" , Odometry , self.odomInput)
rospy.Subscriber("base_bumper/info" , String , self.bumpedInput)
rospy.Subscriber("torso_lift_bumper/info", String , self.bumpedInput)
rospy.Subscriber("/move_base/feedback" , MoveBaseState , self.stateInput)
- rospy.Subscriber("/amcl_pose" , PoseWithCovariance , self.amclInput)
+ rospy.Subscriber("/amcl_pose" , PoseWithCovarianceStamped , self.amclInput)
# below only for debugging build 303, base not moving
rospy.Subscriber("cmd_vel" , PoseDot , self.cmd_velInput)
@@ -313,7 +313,7 @@
q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3])
pose = Pose(p,q)
print "publishing initialpose",h,p,COV[0]
- pub_pose.publish(PoseWithCovariance(h, pose,COV))
+ pub_pose.publish(PoseWithCovarianceStamped(h, PoseWithCovariance(pose,COV)))
else:
# send goal until state /move_base/feedback indicates goal is received
p = Point(self.target_x, self.target_y, 0)
Modified: pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp
===================================================================
--- pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -40,7 +40,7 @@
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <gtest/gtest.h>
using namespace robot_msgs;
@@ -48,12 +48,12 @@
int count=0;
-void on_pose(robot_actions::ActionClient<PoseStamped, PoseStampedState, PoseStamped> &client,PoseWithCovarianceConstPtr pose)
+void on_pose(robot_actions::ActionClient<PoseStamped, PoseStampedState, PoseStamped> &client,PoseWithCovarianceStampedConstPtr pose)
{
ROS_INFO_STREAM("pose "<<count <<","<<pose);
PoseStamped goal;
- goal.pose=pose->pose;
+ goal.pose=pose->data.pose;
goal.header=pose->header;
PoseStamped feedback;
robot_actions::ResultStatus result = client.execute(goal, feedback, ros::Duration(5));
@@ -71,7 +71,7 @@
//duration.sleep();
ros::NodeHandle n_;
- ros::Subscriber s=n_.subscribe<PoseWithCovariance>("amcl_pose",1,boost::bind(&on_pose,client,_1));
+ ros::Subscriber s=n_.subscribe<PoseWithCovarianceStamped>("amcl_pose",1,boost::bind(&on_pose,client,_1));
ros::spin();
Modified: pkg/trunk/sandbox/webteleop/src/server.py
===================================================================
--- pkg/trunk/sandbox/webteleop/src/server.py 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/sandbox/webteleop/src/server.py 2009-08-08 01:30:20 UTC (rev 21110)
@@ -41,7 +41,7 @@
rosCoreUp = False
launchers = {}
-msgClasses = { '/initialpose' : geometry_msgs.msg.PoseWithCovariance,
+msgClasses = { '/initialpose' : geometry_msgs.msg.PoseWithCovarianceStamped,
'/move_base/activate' : geometry_msgs.msg.PoseStamped
}
Modified: pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg 2009-08-08 01:30:20 UTC (rev 21110)
@@ -1,2 +1,2 @@
Header header
-PoseWithCovariance pose_with_covariance
+PoseWithCovariance data
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation.h 2009-08-08 01:30:20 UTC (rev 21110)
@@ -48,7 +48,7 @@
#include <tf/tf.h>
// msgs
-#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
// log files
#include <fstream>
@@ -106,7 +106,7 @@
/** get the filter posterior
* \param estimate the filter posterior as a pose with covariance
*/
- void getEstimate(geometry_msgs::PoseWithCovariance& estimate);
+ void getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate);
/** Add a sensor measurement to the measurement buffer
* \param meas the measurement to add
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h 2009-08-08 01:30:20 UTC (rev 21110)
@@ -50,7 +50,7 @@
#include "geometry_msgs/PoseWithRatesStamped.h"
#include "geometry_msgs/PoseStamped.h"
#include "robot_msgs/VOPose.h"
-#include "geometry_msgs/PoseWithCovariance.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <boost/thread/mutex.hpp>
@@ -108,7 +108,7 @@
OdomEstimation my_filter_;
// estimated robot pose message to send
- geometry_msgs::PoseWithCovariance output_;
+ geometry_msgs::PoseWithCovarianceStamped output_;
// robot state
tf::TransformListener robot_state_;
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -334,8 +334,8 @@
transformer_.lookupTransform("base_footprint","odom", time, estimate);
};
- // get most recent filter posterior as PoseWithCovariance
- void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovariance& estimate)
+ // get most recent filter posterior as PoseWithCovarianceStamped
+ void OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)
{
// pose
Stamped<Transform> tmp;
@@ -344,7 +344,7 @@
return;
}
transformer_.lookupTransform("base_footprint","odom", ros::Time(), tmp);
- poseTFToMsg(tmp, estimate.pose);
+ poseTFToMsg(tmp, estimate.data.pose);
// header
estimate.header.stamp = tmp.stamp_;
@@ -354,7 +354,7 @@
SymmetricMatrix covar = filter_->PostGet()->CovarianceGet();
for (unsigned int i=0; i<6; i++)
for (unsigned int j=0; j<6; j++)
- estimate.covariance[6*i+j] = covar(i+1,j+1);
+ estimate.data.covariance[6*i+j] = covar(i+1,j+1);
};
// correct for angle overflow
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -74,7 +74,7 @@
timer_ = node_.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
// advertise our estimation
- pose_pub_ = node_.advertise<geometry_msgs::PoseWithCovariance>("~"+publish_name, 10);
+ pose_pub_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>("~"+publish_name, 10);
// initialize
filter_stamp_ = Time::now();
@@ -303,7 +303,7 @@
{
// receive data
boost::mutex::scoped_lock lock(vel_mutex_);
- vel_desi_(1) = vel->vel.vx; vel_desi_(2) = vel->ang_vel.vz;
+ vel_desi_(1) = vel->linear.x; vel_desi_(2) = vel->angular.z;
// active
//if (!vel_active_) vel_active_ = true;
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -37,7 +37,7 @@
#include <string>
#include <gtest/gtest.h>
#include "ros/node.h"
-#include "geometry_msgs/PoseWithCovariance.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseWithRatesStamped.h"
@@ -56,7 +56,7 @@
class TestEKF : public testing::Test
{
public:
- geometry_msgs::PoseWithCovariance ekf_msg_, ekf_begin_;
+ geometry_msgs::PoseWithCovarianceStamped ekf_msg_, ekf_begin_;
geometry_msgs::PoseWithRatesStamped odom_msg_;
double ekf_counter_, odom_counter_;
Time ekf_time_begin_, odom_time_begin_;
@@ -142,20 +142,20 @@
EXPECT_GT(Duration(ekf_msg_.header.stamp - odom_msg_.header.stamp).toSec(), -1.0);
// check filter result
- EXPECT_GT(ekf_begin_.pose.position.x, 0.038043 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.position.x, 0.038043 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.position.y, -0.001618 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.position.y, -0.001618 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.position.z, 0.000000 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.position.z, 0.000000 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.orientation.x, 0.000000 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.orientation.x, 0.000000 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.orientation.y, 0.000000 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.orientation.y, 0.000000 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.orientation.z, 0.088400 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.orientation.z, 0.088400 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.orientation.w, 0.996085 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.orientation.w, 0.996085 + EPS_rot);
+ EXPECT_GT(ekf_begin_.data.pose.position.x, 0.038043 - EPS_trans);
+ EXPECT_LT(ekf_begin_.data.pose.position.x, 0.038043 + EPS_trans);
+ EXPECT_GT(ekf_begin_.data.pose.position.y, -0.001618 - EPS_trans);
+ EXPECT_LT(ekf_begin_.data.pose.position.y, -0.001618 + EPS_trans);
+ EXPECT_GT(ekf_begin_.data.pose.position.z, 0.000000 - EPS_trans);
+ EXPECT_LT(ekf_begin_.data.pose.position.z, 0.000000 + EPS_trans);
+ EXPECT_GT(ekf_begin_.data.pose.orientation.x, 0.000000 - EPS_rot);
+ EXPECT_LT(ekf_begin_.data.pose.orientation.x, 0.000000 + EPS_rot);
+ EXPECT_GT(ekf_begin_.data.pose.orientation.y, 0.000000 - EPS_rot);
+ EXPECT_LT(ekf_begin_.data.pose.orientation.y, 0.000000 + EPS_rot);
+ EXPECT_GT(ekf_begin_.data.pose.orientation.z, 0.088400 - EPS_rot);
+ EXPECT_LT(ekf_begin_.data.pose.orientation.z, 0.088400 + EPS_rot);
+ EXPECT_GT(ekf_begin_.data.pose.orientation.w, 0.996085 - EPS_rot);
+ EXPECT_LT(ekf_begin_.data.pose.orientation.w, 0.996085 + EPS_rot);
SUCCEED();
}
Modified: pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp
===================================================================
--- pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -37,7 +37,7 @@
// Messages that I need
#include "sensor_msgs/LaserScan.h"
-#include "geometry_msgs/PoseWithCovariance.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "nav_msgs/GetMap.h"
@@ -106,7 +106,7 @@
static pf_vector_t uniformPoseGenerator(void* arg);
// incoming messages
- geometry_msgs::PoseWithCovariance initial_pose_;
+ geometry_msgs::PoseWithCovarianceStamped initial_pose_;
// Message callbacks
bool globalLocalizationCallback(std_srvs::Empty::Request& req,
@@ -126,7 +126,7 @@
ros::Time save_pose_last_time;
ros::Duration save_pose_period;
- geometry_msgs::PoseWithCovariance last_published_pose;
+ geometry_msgs::PoseWithCovarianceStamped last_published_pose;
map_t* map_;
char* mapdata;
@@ -319,7 +319,7 @@
laser_->SetModelLikelihoodField(z_hit, z_rand, sigma_hit,
laser_likelihood_max_dist);
- ros::Node::instance()->advertise<geometry_msgs::PoseWithCovariance>("amcl_pose",2);
+ ros::Node::instance()->advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",2);
ros::Node::instance()->advertise<geometry_msgs::PoseArray>("particlecloud",2);
ros::Node::instance()->advertise<visualization_msgs::Polyline>("gui_laser",2);
ros::Node::instance()->advertiseService("global_localization",
@@ -664,15 +664,15 @@
puts("");
*/
- geometry_msgs::PoseWithCovariance p;
+ geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = "map";
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
- p.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
- p.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
+ p.data.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
+ p.data.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::Quaternion(hyps[max_weight_hyp].pf_pose_mean.v[2], 0.0, 0.0),
- p.pose.orientation);
+ p.data.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
@@ -682,13 +682,13 @@
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
- p.covariance[6*i+j] = set->cov.m[i][j];
+ p.data.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*3+3] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
- p.covariance[6*3+3] = set->cov.m[2][2];
+ p.data.covariance[6*3+3] = set->cov.m[2][2];
/*
printf("cov:\n");
@@ -821,11 +821,11 @@
ros::Node::instance()->setParam("~initial_pose_y", map_pose.getOrigin().y());
ros::Node::instance()->setParam("~initial_pose_a", yaw);
ros::Node::instance()->setParam("~initial_cov_xx",
- last_published_pose.covariance[6*0+0]);
+ last_published_pose.data.covariance[6*0+0]);
ros::Node::instance()->setParam("~initial_cov_yy",
- last_published_pose.covariance[6*1+1]);
+ last_published_pose.data.covariance[6*1+1]);
ros::Node::instance()->setParam("~initial_cov_aa",
- last_published_pose.covariance[6*3+3]);
+ last_published_pose.data.covariance[6*3+3]);
save_pose_last_time = now;
}
}
@@ -861,7 +861,7 @@
}
tf::Pose pose_old, pose_new;
- tf::poseMsgToTF(initial_pose_.pose, pose_old);
+ tf::poseMsgToTF(initial_pose_.data.pose, pose_old);
pose_new = tx_odom.inverse() * pose_old;
ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",
@@ -880,10 +880,10 @@
{
for(int j=0; j<2; j++)
{
- pf_init_pose_cov.m[i][j] = initial_pose_.covariance[6*i+j];
+ pf_init_pose_cov.m[i][j] = initial_pose_.data.covariance[6*i+j];
}
}
- pf_init_pose_cov.m[2][2] = initial_pose_.covariance[6*3+3];
+ pf_init_pose_cov.m[2][2] = initial_pose_.data.covariance[6*3+3];
pf_mutex_.lock();
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
Modified: pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -59,7 +59,7 @@
- @b "base_pose_ground_truth" geometry_msgs/PoseWithRatesStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
-- @b "amcl_pose" geometry_msgs/PoseWithCovariance : robot's estimated pose in the map, with covariance
+- @b "amcl_pose" geometry_msgs/PoseWithCovarianceStamped : robot's estimated pose in the map, with covariance
- @b "particlecloud" geometry_msgs/PoseArray : fake set of poses being maintained by the filter (one paricle only).
<hr>
@@ -75,7 +75,7 @@
#include <geometry_msgs/PoseWithRatesStamped.h>
#include <geometry_msgs/PoseArray.h>
-#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <angles/angles.h>
@@ -91,7 +91,7 @@
public:
FakeOdomNode(void) : ros::Node("fake_localization")
{
- advertise<geometry_msgs::PoseWithCovariance>("amcl_pose",1);
+ advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose",1);
advertise<geometry_msgs::PoseArray>("particlecloud",1);
m_tfServer = new tf::TransformBroadcaster();
m_tfListener = new tf::TransformListener(*this);
@@ -141,7 +141,7 @@
geometry_msgs::PoseWithRatesStamped m_basePosMsg;
geometry_msgs::PoseArray m_particleCloud;
- geometry_msgs::PoseWithCovariance m_currentPos;
+ geometry_msgs::PoseWithCovarianceStamped m_currentPos;
//parameter for what odom to use
std::string odom_frame_id_;
@@ -205,16 +205,16 @@
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = "/map"; ///\todo fixme hack
- m_currentPos.pose.position.x = x;
- m_currentPos.pose.position.y = y;
+ m_currentPos.data.pose.position.x = x;
+ m_currentPos.data.pose.position.y = y;
// Leave z as zero
tf::quaternionTFToMsg(tf::Quaternion(yaw, 0.0, 0.0),
- m_currentPos.pose.orientation);
+ m_currentPos.data.pose.orientation);
// Leave covariance as zero
publish("amcl_pose", m_currentPos);
// The particle cloud is the current position. Quite convenient.
- m_particleCloud.poses[0] = m_currentPos.pose;
+ m_particleCloud.poses[0] = m_currentPos.data.pose;
publish("particlecloud", m_particleCloud);
}
};
Modified: pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/navigation/nav_view/src/nav_view/nav_view_panel.h 2009-08-08 01:30:20 UTC (rev 21110)
@@ -35,7 +35,7 @@
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/PoseStamped.h"
#include "visualization_msgs/Polyline.h"
-#include "geometry_msgs/PoseWithCovariance.h"
+#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <OGRE/OgreTexture.h>
#include <OGRE/OgreMaterial.h>
Modified: pkg/trunk/stacks/navigation/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/stacks/navigation/nav_view/src/nav_view/tools.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/navigation/nav_view/src/nav_view/tools.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -88,7 +88,7 @@
, is_goal_( goal )
{
goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("goal", 1);
- pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovariance>("initialpose", 1);
+ pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
}
PoseTool::~PoseTool()
@@ -171,14 +171,14 @@
}
else
{
- geometry_msgs::PoseWithCovariance pose;
- pose.pose.position.x = pos_.x;
- pose.pose.position.y = pos_.y;
+ geometry_msgs::PoseWithCovarianceStamped pose;
+ pose.data.pose.position.x = pos_.x;
+ pose.data.pose.position.y = pos_.y;
tf::quaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
- pose.pose.orientation);
- pose.covariance[6*0+0] = 0.5 * 0.5;
- pose.covariance[6*1+1] = 0.5 * 0.5;
- pose.covariance[6*3+3] = M_PI/12.0 * M_PI/12.0;
+ pose.data.pose.orientation);
+ pose.data.covariance[6*0+0] = 0.5 * 0.5;
+ pose.data.covariance[6*1+1] = 0.5 * 0.5;
+ pose.data.covariance[6*3+3] = M_PI/12.0 * M_PI/12.0;
ROS_INFO( "setting pose: %.3f %.3f %.3f\n", pos_.x, pos_.y, angle );
pose_pub_.publish( pose );
}
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-08-08 01:30:20 UTC (rev 21110)
@@ -37,7 +37,7 @@
#include "ogre_tools/wx_ogre_render_window.h"
#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <OGRE/OgreRay.h>
#include <OGRE/OgrePlane.h>
@@ -61,7 +61,7 @@
arrow_->getSceneNode()->setVisible( false );
goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("goal", 1);
- pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovariance>("initialpose", 1);
+ pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1);
}
PoseTool::~PoseTool()
@@ -159,14 +159,14 @@
}
else
{
- geometry_msgs::PoseWithCovariance pose;
- pose.pose.position.x = robot_pos_transformed.x();
- pose.pose.position.y = robot_pos_transformed.y();
+ geometry_msgs::PoseWithCovarianceStamped pose;
+ pose.data.pose.position.x = robot_pos_transformed.x();
+ pose.data.pose.position.y = robot_pos_transformed.y();
tf::quaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.0),
- pose.pose.orientation);
- pose.covariance[6*0+0] = 0.5 * 0.5;
- pose.covariance[6*1+1] = 0.5 * 0.5;
- pose.covariance[6*3+3] = M_PI/12.0 * M_PI/12.0;
+ pose.data.pose.orientation);
+ pose.data.covariance[6*0+0] = 0.5 * 0.5;
+ pose.data.covariance[6*1+1] = 0.5 * 0.5;
+ pose.data.covariance[6*3+3] = M_PI/12.0 * M_PI/12.0;
ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]",
robot_pos_transformed.x(),
robot_pos_transformed.y(),
Modified: pkg/trunk/vision/vslam/nodes/picmap.py
===================================================================
--- pkg/trunk/vision/vslam/nodes/picmap.py 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/vision/vslam/nodes/picmap.py 2009-08-08 01:30:20 UTC (rev 21110)
@@ -107,7 +107,7 @@
#self.pub = rospy.Publisher("/picmap_pose", vslam.msg.Picmap)
rospy.Subscriber('/stereo/raw_stereo', sensor_msgs.msg.RawStereo, self.handle_raw_stereo_queue, queue_size=2, buff_size=7000000)
- rospy.Subscriber('/amcl_pose', robot_msgs.msg.PoseWithCovariance, self.handle_amcl_pose)
+ rospy.Subscriber('/amcl_pose', robot_msgs.msg.PoseWithCovarianceStamped, self.handle_amcl_pose)
rospy.Subscriber('/tf_message', tf.msg.tfMessage, self.handle_tf_queue)
self.pmlock = threading.Lock()
@@ -191,8 +191,8 @@
def handle_amcl_pose(self, msg):
if self.vo:
print "handle_amcl_pose(", msg, ")"
- happy = max([msg.covariance[0], msg.covariance[7]]) < 0.003
- print "picmap node got amcl", msg.header.stamp.to_seconds(), msg.covariance[0], msg.covariance[7], "happy", happy
+ happy = max([msg.data.covariance[0], msg.data.covariance[7]]) < 0.003
+ print "picmap node got amcl", msg.header.stamp.to_seconds(), msg.data.covariance[0], msg.data.covariance[7], "happy", happy
self.pmlock.acquire()
self.pm.newLocalization(msg.header.stamp.to_seconds(), happy)
self.pmlock.release()
Modified: pkg/trunk/vision/vslam/nodes/roadmap.py
===================================================================
--- pkg/trunk/vision/vslam/nodes/roadmap.py 2009-08-08 01:21:58 UTC (rev 21109)
+++ pkg/trunk/vision/vslam/nodes/roadmap.py 2009-08-08 01:30:20 UTC (rev 21110)
@@ -246,7 +246,7 @@
def __init__(self, args):
rospy.init_node('roadmap_server')
self.pub = rospy.Publisher("roadmap", vslam.msg.Roadmap)
- rospy.Subscriber('amcl_pose', robot_msgs.msg.PoseWithCovariance, self.handle_localizedpose)
+ rospy.Subscriber('amcl_pose', robot_msgs.msg.PoseWithCovarianceStamped, self.handle_localizedpose)
self.updated = None
rospy.Subscriber('time', roslib.msg.Time, self.handle_time)
self.nodes = []
@@ -259,9 +259,9 @@
self.send_map(msg.header.stamp)
def handle_localizedpose(self, msg):
- th = tf.transformations.euler_from_quaternion([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])[2]
- x = msg.pose.position.x
- y = msg.pose.position.y
+ th = tf.transformations.euler_from_quaternion([msg.data.pose.orientation.x, msg.data.pose.orientation.y, msg.data.pose.orientation.z, msg.data.pose.orientation.w])[2]
+ x = msg.data.pose.position.x
+ y = msg.data.pose.position.y
print x, y, th
n = (x, y, th)
if self.nodes == [] or (dist(self.nodes[-1], n) > 1.0) or (abs(self.nodes[-1][2] - n[2]) > (2.0 * pi / 180)):
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
