Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21151] pkg/trunk
|
From: <jfa...@us...> - 2009-08-08 06:00:43
|
Revision: 21151
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21151&view=rev
Author: jfaustwg
Date: 2009-08-08 06:00:31 +0000 (Sat, 08 Aug 2009)
Log Message:
-----------
PoseWithCovarianceStamped::data -> PoseWithCovarianceStamped::pose
Modified Paths:
--------------
pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.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/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/sandbox/labeled_object_detector/test/test_nearest_action2.cpp
===================================================================
--- pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp 2009-08-08 05:59:29 UTC (rev 21150)
+++ pkg/trunk/sandbox/labeled_object_detector/test/test_nearest_action2.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -53,7 +53,7 @@
ROS_INFO_STREAM("pose "<<count <<","<<pose);
PoseStamped goal;
- goal.pose=pose->data.pose;
+ goal.pose=pose->pose.pose;
goal.header=pose->header;
PoseStamped feedback;
robot_actions::ResultStatus result = client.execute(goal, feedback, ros::Duration(5));
Modified: pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg 2009-08-08 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg 2009-08-08 06:00:31 UTC (rev 21151)
@@ -1,2 +1,2 @@
Header header
-PoseWithCovariance data
+PoseWithCovariance pose
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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -344,7 +344,7 @@
return;
}
transformer_.lookupTransform("base_footprint","odom", ros::Time(), tmp);
- poseTFToMsg(tmp, estimate.data.pose);
+ poseTFToMsg(tmp, estimate.pose.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.data.covariance[6*i+j] = covar(i+1,j+1);
+ estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
};
// correct for angle overflow
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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -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_.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);
+ EXPECT_GT(ekf_begin_.pose.pose.position.x, 0.038043 - EPS_trans);
+ EXPECT_LT(ekf_begin_.pose.pose.position.x, 0.038043 + EPS_trans);
+ EXPECT_GT(ekf_begin_.pose.pose.position.y, -0.001618 - EPS_trans);
+ EXPECT_LT(ekf_begin_.pose.pose.position.y, -0.001618 + EPS_trans);
+ EXPECT_GT(ekf_begin_.pose.pose.position.z, 0.000000 - EPS_trans);
+ EXPECT_LT(ekf_begin_.pose.pose.position.z, 0.000000 + EPS_trans);
+ EXPECT_GT(ekf_begin_.pose.pose.orientation.x, 0.000000 - EPS_rot);
+ EXPECT_LT(ekf_begin_.pose.pose.orientation.x, 0.000000 + EPS_rot);
+ EXPECT_GT(ekf_begin_.pose.pose.orientation.y, 0.000000 - EPS_rot);
+ EXPECT_LT(ekf_begin_.pose.pose.orientation.y, 0.000000 + EPS_rot);
+ EXPECT_GT(ekf_begin_.pose.pose.orientation.z, 0.088400 - EPS_rot);
+ EXPECT_LT(ekf_begin_.pose.pose.orientation.z, 0.088400 + EPS_rot);
+ EXPECT_GT(ekf_begin_.pose.pose.orientation.w, 0.996085 - EPS_rot);
+ EXPECT_LT(ekf_begin_.pose.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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/navigation/amcl/src/amcl_node.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -669,10 +669,10 @@
p.header.frame_id = "map";
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
- 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];
+ p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
+ p.pose.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.data.pose.orientation);
+ p.pose.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.data.covariance[6*i+j] = set->cov.m[i][j];
+ p.pose.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.data.covariance[6*3+3] = set->cov.m[2][2];
+ p.pose.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.data.covariance[6*0+0]);
+ last_published_pose.pose.covariance[6*0+0]);
ros::Node::instance()->setParam("~initial_cov_yy",
- last_published_pose.data.covariance[6*1+1]);
+ last_published_pose.pose.covariance[6*1+1]);
ros::Node::instance()->setParam("~initial_cov_aa",
- last_published_pose.data.covariance[6*3+3]);
+ last_published_pose.pose.covariance[6*3+3]);
save_pose_last_time = now;
}
}
@@ -861,7 +861,7 @@
}
tf::Pose pose_old, pose_new;
- tf::poseMsgToTF(initial_pose_.data.pose, pose_old);
+ tf::poseMsgToTF(initial_pose_.pose.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_.data.covariance[6*i+j];
+ pf_init_pose_cov.m[i][j] = initial_pose_.pose.covariance[6*i+j];
}
}
- pf_init_pose_cov.m[2][2] = initial_pose_.data.covariance[6*3+3];
+ pf_init_pose_cov.m[2][2] = initial_pose_.pose.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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -205,16 +205,16 @@
// Publish localized pose
m_currentPos.header = message->header;
m_currentPos.header.frame_id = "/map"; ///\todo fixme hack
- m_currentPos.data.pose.position.x = x;
- m_currentPos.data.pose.position.y = y;
+ m_currentPos.pose.pose.position.x = x;
+ m_currentPos.pose.pose.position.y = y;
// Leave z as zero
tf::quaternionTFToMsg(tf::Quaternion(yaw, 0.0, 0.0),
- m_currentPos.data.pose.orientation);
+ m_currentPos.pose.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.data.pose;
+ m_particleCloud.poses[0] = m_currentPos.pose.pose;
publish("particlecloud", m_particleCloud);
}
};
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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/navigation/nav_view/src/nav_view/tools.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -172,13 +172,13 @@
else
{
geometry_msgs::PoseWithCovarianceStamped pose;
- pose.data.pose.position.x = pos_.x;
- pose.data.pose.position.y = pos_.y;
+ pose.pose.pose.position.x = pos_.x;
+ pose.pose.pose.position.y = pos_.y;
tf::quaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.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;
+ pose.pose.pose.orientation);
+ pose.pose.covariance[6*0+0] = 0.5 * 0.5;
+ pose.pose.covariance[6*1+1] = 0.5 * 0.5;
+ pose.pose.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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/tools/pose_tool.cpp 2009-08-08 06:00:31 UTC (rev 21151)
@@ -160,13 +160,13 @@
else
{
geometry_msgs::PoseWithCovarianceStamped pose;
- pose.data.pose.position.x = robot_pos_transformed.x();
- pose.data.pose.position.y = robot_pos_transformed.y();
+ pose.pose.pose.position.x = robot_pos_transformed.x();
+ pose.pose.pose.position.y = robot_pos_transformed.y();
tf::quaternionTFToMsg(tf::Quaternion(angle, 0.0, 0.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;
+ pose.pose.pose.orientation);
+ pose.pose.covariance[6*0+0] = 0.5 * 0.5;
+ pose.pose.covariance[6*1+1] = 0.5 * 0.5;
+ pose.pose.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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/vision/vslam/nodes/picmap.py 2009-08-08 06:00:31 UTC (rev 21151)
@@ -191,8 +191,8 @@
def handle_amcl_pose(self, msg):
if self.vo:
print "handle_amcl_pose(", msg, ")"
- 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
+ happy = max([msg.pose.covariance[0], msg.pose.covariance[7]]) < 0.003
+ print "picmap node got amcl", msg.header.stamp.to_seconds(), msg.pose.covariance[0], msg.pose.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 05:59:29 UTC (rev 21150)
+++ pkg/trunk/vision/vslam/nodes/roadmap.py 2009-08-08 06:00:31 UTC (rev 21151)
@@ -259,9 +259,9 @@
self.send_map(msg.header.stamp)
def handle_localizedpose(self, msg):
- 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
+ th = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])[2]
+ x = msg.pose.pose.position.x
+ y = msg.pose.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.
|
