Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21278] pkg/trunk
|
From: <sf...@us...> - 2009-08-10 00:02:43
|
Revision: 21278
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21278&view=rev
Author: sfkwc
Date: 2009-08-10 00:02:33 +0000 (Mon, 10 Aug 2009)
Log Message:
-----------
#2250 getting rid of _with_covariance in Odometry fields
Modified Paths:
--------------
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
pkg/trunk/stacks/common_msgs/nav_msgs/msg/Odometry.msg
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
pkg/trunk/stacks/simulators/stage/src/stageros.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
pkg/trunk/util/logsetta/odom/odom_extract.cpp
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -72,12 +72,12 @@
{
_odom_mutex.lock();
if (!_odom_active){
- _odom_begin = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
- _odom_end = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
+ _odom_begin = tf::getYaw(_odom.pose.pose.orientation);
+ _odom_end = tf::getYaw(_odom.pose.pose.orientation);
_odom_active = true;
}
else{
- double tmp = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
+ double tmp = tf::getYaw(_odom.pose.pose.orientation);
AngleOverflowCorrect(tmp, _odom_end);
_odom_end = tmp;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -155,17 +155,17 @@
{
msg.header.frame_id = odom_frame_;
msg.header.stamp.fromSec(current_time_);
- msg.pose_with_covariance.pose.position.x = odom_.x;
- msg.pose_with_covariance.pose.position.y = odom_.y;
- msg.pose_with_covariance.pose.position.z = 0.0;
+ msg.pose.pose.position.x = odom_.x;
+ msg.pose.pose.position.y = odom_.y;
+ msg.pose.pose.position.z = 0.0;
tf::Quaternion quat_trans = tf::Quaternion(odom_.z, 0.0, 0.0);
- msg.pose_with_covariance.pose.orientation.x = quat_trans.x();
- msg.pose_with_covariance.pose.orientation.y = quat_trans.y();
- msg.pose_with_covariance.pose.orientation.z = quat_trans.z();
- msg.pose_with_covariance.pose.orientation.w = quat_trans.w();
+ msg.pose.pose.orientation.x = quat_trans.x();
+ msg.pose.pose.orientation.y = quat_trans.y();
+ msg.pose.pose.orientation.z = quat_trans.z();
+ msg.pose.pose.orientation.w = quat_trans.w();
- msg.twist_with_covariance.twist = odom_vel_;
+ msg.twist.twist = odom_vel_;
#warning A full covariance should be published instead of the residual, this code will not work properly with the ekf until this is changed
//@todo TODO: change from residual to covariance
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -162,9 +162,9 @@
if(run_time_set && delta_time.toSec() > run_time)
break;
- odom_log_file << tb.odom.pose_with_covariance.pose.position.x << " " << tb.odom.pose_with_covariance.pose.position.y << " " << tf::getYaw(tb.odom.pose_with_covariance.pose.orientation) << " " << tb.odom.twist_with_covariance.twist.linear.x << " " << tb.odom.twist_with_covariance.twist.linear.y << " " << tb.odom.twist_with_covariance.twist.angulat.z << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+ odom_log_file << tb.odom.pose.pose.position.x << " " << tb.odom.pose.pose.position.y << " " << tf::getYaw(tb.odom.pose.pose.orientation) << " " << tb.odom.twist.twist.linear.x << " " << tb.odom.twist.twist.linear.y << " " << tb.odom.twist.twist.angulat.z << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
- cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.twist_with_covariance.twist.linear.x << endl << " y:" << tb.odom.twist_with_covariance.twist.linear.y << endl << " omega:" << tb.odom.twist_with_covariance.twist.angular.z << std::endl;
+ cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.twist.twist.linear.x << endl << " y:" << tb.odom.twist.twist.linear.y << endl << " omega:" << tb.odom.twist.twist.angular.z << std::endl;
node->publish("cmd_vel",cmd);
sleep_time.sleep();
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -172,9 +172,9 @@
if(run_time_set && delta_time.toSec() > run_time)
break;
// ang_rates = GetAsEuler(tb.ground_truth.vel.ang_vel);
- ground_truth_angles = GetAsEuler(tb.ground_truth.pose_with_covariance.pose.orientation);
- cout << "g:: " << tb.ground_truth.twist_with_covariance.twist.linear.x << " " << tb.ground_truth.twist_with_covariance.twist.linear.y << " " << tb.ground_truth.twist_with_covariance.twist.angular.z << " " << tb.ground_truth.pose_with_covariance.pose.position.x << " " << tb.ground_truth.pose_with_covariance.pose.position.y << " " << ground_truth_angles.z << " " << tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
- cout << "o:: " << tb.odom.twist_with_covariance.twist.linear.x << " " << tb.odom.twist_with_covariance.twist.linear.y << " " << tb.odom.twist_with_covariance.twist.angular.z << " " << tb.odom.pose_with_covariance.pose.position.x << " " << tb.odom.pose_with_covariance.pose.position.y << " " << tf::getYaw(tb.odom.pose_with_covariance.pose.orientation) << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+ ground_truth_angles = GetAsEuler(tb.ground_truth.pose.pose.orientation);
+ cout << "g:: " << tb.ground_truth.twist.twist.linear.x << " " << tb.ground_truth.twist.twist.linear.y << " " << tb.ground_truth.twist.twist.angular.z << " " << tb.ground_truth.pose.pose.position.x << " " << tb.ground_truth.pose.pose.position.y << " " << ground_truth_angles.z << " " << tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+ cout << "o:: " << tb.odom.twist.twist.linear.x << " " << tb.odom.twist.twist.linear.y << " " << tb.odom.twist.twist.angular.z << " " << tb.odom.pose.pose.position.x << " " << tb.odom.pose.pose.position.y << " " << tf::getYaw(tb.odom.pose.pose.orientation) << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
// cout << delta_time.toSec() << " " << run_time << endl;
node->publish("cmd_vel",cmd);
sleep_time.sleep();
Modified: pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -154,14 +154,14 @@
# initialize odom
if self.odom_initialized == False or self.p3d_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
- self.odom_qi = quaternionMsgToList(odom.pose_with_covariance.pose.orientation)
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
+ self.odom_qi = quaternionMsgToList(odom.pose.pose.orientation)
else:
# update odom
- self.odom_x = odom.pose_with_covariance.pose.position.x
- self.odom_y = odom.pose_with_covariance.pose.position.y
- self.odom_q = quaternionMsgToList(odom.pose_with_covariance.pose.orientation)
+ self.odom_x = odom.pose.pose.position.x
+ self.odom_y = odom.pose.pose.position.y
+ self.odom_q = quaternionMsgToList(odom.pose.pose.orientation)
def p3dInput(self, p3d):
#self.printBaseP3D(p3d)
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -68,9 +68,9 @@
def tfCallback(self,msg):
if (msg.transforms[0].header.frame_id == self.global_frame and
self.odom_pose != None):
- self.robot_position = Point((self.odom_pose.pose_with_covariance.pose.position.x -
+ self.robot_position = Point((self.odom_pose.pose.pose.position.x -
msg.transforms[0].transform.translation.x),
- (self.odom_pose.pose_with_covariance.pose.position.y -
+ (self.odom_pose.pose.pose.position.y -
msg.transforms[0].transform.translation.y),
0.0)
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -154,14 +154,14 @@
# initialize odom
if self.odom_initialized == False or self.p3d_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
- self.odom_qi = quaternionMsgToList(odom.pose_with_covariance.orientation)
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
+ self.odom_qi = quaternionMsgToList(odom.pose.orientation)
else:
# update odom
self.odom_x = odom.pos.x
self.odom_y = odom.pos.y
- self.odom_q = quaternionMsgToList(odom.pose_with_covariance.orientation)
+ self.odom_q = quaternionMsgToList(odom.pose.orientation)
def p3dInput(self, p3d):
#self.printBaseP3D(p3d)
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -114,12 +114,12 @@
m_currentPos.header = snapshot_.header;
m_currentPos.header.frame_id = "map";
- m_currentPos.pose_with_covariance.pose.position.x = body.pose.translation.x;
- m_currentPos.pose_with_covariance.pose.position.y = body.pose.translation.y;
+ m_currentPos.pose.pose.position.x = body.pose.translation.x;
+ m_currentPos.pose.pose.position.y = body.pose.translation.y;
double yaw,pitch,roll;
mytf.getBasis().getEulerZYX(yaw,pitch,roll);
- m_currentPos.pose_with_covariance.pose.orientation= tf::createQuaternionMsgFromYaw(yaw);
+ m_currentPos.pose.pose.orientation= tf::createQuaternionMsgFromYaw(yaw);
if (publish_localized_pose_)
publish("localizedpose", m_currentPos) ;
@@ -127,10 +127,10 @@
// Build Ground Truth Message
nav_msgs::Odometry m_ground_truth ;
m_ground_truth.header = snapshot_.header ;
- m_ground_truth.pose_with_covariance.pose.position.x = body.pose.translation.x ;
- m_ground_truth.pose_with_covariance.pose.position.y = body.pose.translation.y ;
- m_ground_truth.pose_with_covariance.pose.position.z = body.pose.translation.z ;
- m_ground_truth.pose_with_covariance.pose.orientation = body.pose.rotation ;
+ m_ground_truth.pose.pose.position.x = body.pose.translation.x ;
+ m_ground_truth.pose.pose.position.y = body.pose.translation.y ;
+ m_ground_truth.pose.pose.position.z = body.pose.translation.z ;
+ m_ground_truth.pose.pose.orientation = body.pose.rotation ;
publish("base_pose_ground_truth", m_ground_truth) ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -125,12 +125,12 @@
tfb.sendTransform(tmp_tf_stamped);
- odom.pose_with_covariance.pose.position.x = this->posIface->data->pose.pos.x;
- odom.pose_with_covariance.pose.position.y = this->posIface->data->pose.pos.y;
- odom.pose_with_covariance.pose.orientation = tf::createQuaternionMsgFromYaw(this->posIface->data->pose.yaw);
- odom.twist_with_covariance.twist.linear.x = this->posIface->data->velocity.pos.x;
- odom.twist_with_covariance.twist.linear.y = this->posIface->data->velocity.pos.y;
- odom.twist_with_covariance.twist.angular.z = this->posIface->data->velocity.yaw;
+ odom.pose.pose.position.x = this->posIface->data->pose.pos.x;
+ odom.pose.pose.position.y = this->posIface->data->pose.pos.y;
+ odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->posIface->data->pose.yaw);
+ odom.twist.twist.linear.x = this->posIface->data->velocity.pos.x;
+ odom.twist.twist.linear.y = this->posIface->data->velocity.pos.y;
+ odom.twist.twist.angular.z = this->posIface->data->velocity.yaw;
odom.header.frame_id = "odom";
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -149,22 +149,22 @@
// pose is given in inertial frame for Gazebo, transform to the designated frame name
- this->poseMsg.pose_with_covariance.pose.position.x = pos.x;
- this->poseMsg.pose_with_covariance.pose.position.y = pos.y;
- this->poseMsg.pose_with_covariance.pose.position.z = pos.z;
+ this->poseMsg.pose.pose.position.x = pos.x;
+ this->poseMsg.pose.pose.position.y = pos.y;
+ this->poseMsg.pose.pose.position.z = pos.z;
- this->poseMsg.pose_with_covariance.pose.orientation.x = rot.x;
- this->poseMsg.pose_with_covariance.pose.orientation.y = rot.y;
- this->poseMsg.pose_with_covariance.pose.orientation.z = rot.z;
- this->poseMsg.pose_with_covariance.pose.orientation.w = rot.u;
+ this->poseMsg.pose.pose.orientation.x = rot.x;
+ this->poseMsg.pose.pose.orientation.y = rot.y;
+ this->poseMsg.pose.pose.orientation.z = rot.z;
+ this->poseMsg.pose.pose.orientation.w = rot.u;
- this->poseMsg.twist_with_covariance.twist.linear.x = vpos.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.twist_with_covariance.twist.linear.y = vpos.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.twist_with_covariance.twist.linear.z = vpos.z + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.linear.x = vpos.x + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.linear.y = vpos.y + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.linear.z = vpos.z + this->GaussianKernel(0,this->gaussianNoise) ;
// pass euler anglular rates
- this->poseMsg.twist_with_covariance.twist.angular.x = veul.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.twist_with_covariance.twist.angular.y = veul.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.twist_with_covariance.twist.angular.z = veul.z + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.angular.x = veul.x + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.angular.y = veul.y + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->poseMsg.twist.twist.angular.z = veul.z + this->GaussianKernel(0,this->gaussianNoise) ;
// publish to ros
this->pub_.publish(this->poseMsg);
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -129,8 +129,8 @@
this->lock.lock();
Model* model = gazebo::World::Instance()->GetModelByName(this->modelName);
- Vector3 pos(poseMsg->pose_with_covariance.pose.position.x,poseMsg->pose_with_covariance.pose.position.y,poseMsg->pose_with_covariance.pose.position.z);
- Quatern rot(poseMsg->pose_with_covariance.pose.orientation.w,poseMsg->pose_with_covariance.pose.orientation.x,poseMsg->pose_with_covariance.pose.orientation.y,poseMsg->pose_with_covariance.pose.orientation.z);
+ Vector3 pos(poseMsg->pose.pose.position.x,poseMsg->pose.pose.position.y,poseMsg->pose.pose.position.z);
+ Quatern rot(poseMsg->pose.pose.orientation.w,poseMsg->pose.pose.orientation.x,poseMsg->pose.pose.orientation.y,poseMsg->pose.pose.orientation.z);
Pose3d modelPose(pos,rot);
model->SetPose(modelPose);
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -154,16 +154,16 @@
return self.normalize_angle(angle_diff)
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -185,16 +185,16 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
if self.odom_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
self.odom_ei = q.getEuler()
- self.odom_x = odom.pose_with_covariance.pose.position.x - self.odom_xi
- self.odom_y = odom.pose_with_covariance.pose.position.y - self.odom_yi
+ self.odom_x = odom.pose.pose.position.x - self.odom_xi
+ self.odom_y = odom.pose.pose.position.y - self.odom_yi
self.odom_e.shortest_euler_distance(self.odom_ei, q.getEuler())
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -120,16 +120,16 @@
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -149,16 +149,16 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
if self.odom_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
self.odom_ti = q.getEuler().z
- self.odom_x = odom.pose_with_covariance.pose.position.x - self.odom_xi
- self.odom_y = odom.pose_with_covariance.pose.position.y - self.odom_yi
+ self.odom_x = odom.pose.pose.position.x - self.odom_xi
+ self.odom_y = odom.pose.pose.position.y - self.odom_yi
self.odom_t = q.getEuler().z - self.odom_ti
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -119,16 +119,16 @@
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -148,16 +148,16 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
if self.odom_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
self.odom_ti = q.getEuler().z
- self.odom_x = odom.pose_with_covariance.pose.position.x - self.odom_xi
- self.odom_y = odom.pose_with_covariance.pose.position.y - self.odom_yi
+ self.odom_x = odom.pose.pose.position.x - self.odom_xi
+ self.odom_y = odom.pose.pose.position.y - self.odom_yi
self.odom_t = q.getEuler().z - self.odom_ti
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -154,16 +154,16 @@
return self.normalize_angle(angle_diff)
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -187,11 +187,11 @@
#self.printBaseOdom(odom)
if self.odom_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
self.odom_ei = q.getEuler()
- self.odom_x = odom.pose_with_covariance.pose.position.x - self.odom_xi
- self.odom_y = odom.pose_with_covariance.pose.position.y - self.odom_yi
+ self.odom_x = odom.pose.pose.position.x - self.odom_xi
+ self.odom_y = odom.pose.pose.position.y - self.odom_yi
self.odom_e.shortest_euler_distance(self.odom_ei, q.getEuler())
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -119,16 +119,16 @@
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -148,16 +148,16 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
if self.odom_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pose_with_covariance.pose.position.x
- self.odom_yi = odom.pose_with_covariance.pose.position.y
+ self.odom_xi = odom.pose.pose.position.x
+ self.odom_yi = odom.pose.pose.position.y
self.odom_ti = q.getEuler().z
- self.odom_x = odom.pose_with_covariance.pose.position.x - self.odom_xi
- self.odom_y = odom.pose_with_covariance.pose.position.y - self.odom_yi
+ self.odom_x = odom.pose.pose.position.x - self.odom_xi
+ self.odom_y = odom.pose.pose.position.y - self.odom_yi
self.odom_t = q.getEuler().z - self.odom_ti
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py 2009-08-10 00:02:33 UTC (rev 21278)
@@ -68,16 +68,16 @@
def printBaseOdom(self, odom):
- orientation = odom.pose_with_covariance.orientation
+ orientation = odom.pose.orientation
q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pose_with_covariance.pose.position.x)
- print "odom pos " + "y: " + str(odom.pose_with_covariance.pose.position.y)
+ print "odom pos " + "x: " + str(odom.pose.pose.position.x)
+ print "odom pos " + "y: " + str(odom.pose.pose.position.y)
print "odom pos " + "t: " + str(q.getEuler().z)
- print "odom vel " + "x: " + str(odom.twist_with_covariance.twist.linear.x)
- print "odom vel " + "y: " + str(odom.twist_with_covariance.twist.linear.y)
- print "odom vel " + "t: " + str(odom.twist_with_covariance.twist.angular.z)
+ print "odom vel " + "x: " + str(odom.twist.twist.linear.x)
+ print "odom vel " + "y: " + str(odom.twist.twist.linear.y)
+ print "odom vel " + "t: " + str(odom.twist.twist.angular.z)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
Modified: pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -117,7 +117,7 @@
tf::Transform ground_truth_pose ;
- tf::poseMsgToTF(m_ground_truth_.pose_with_covariance.pose, ground_truth_pose) ;
+ tf::poseMsgToTF(m_ground_truth_.pose.pose, ground_truth_pose) ;
//! \todo Compute yaw angle in a more stable way
double yaw,pitch,roll ;
Modified: pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp
===================================================================
--- pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/sandbox/teleop_anti_collision/src/anti_collision_base_controller.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -298,11 +298,11 @@
base_odom_.lock();
try
{
- tf::Stamped<btVector3> v_in(btVector3(msg->twist_with_covariance.twist.linear.x, msg->twist_with_covariance.twist.linear.y, 0), ros::Time(), msg->header.frame_id), v_out;
+ tf::Stamped<btVector3> v_in(btVector3(msg->twist.twist.linear.x, msg->twist.twist.linear.y, 0), ros::Time(), msg->header.frame_id), v_out;
tf_.transformVector(robot_base_frame_, ros::Time(), v_in, msg->header.frame_id, v_out);
base_odom_.linear.x = v_in.x();
base_odom_.linear.y = v_in.y();
- base_odom_.angular.z = msg->twist_with_covariance.twist.angular.z;
+ base_odom_.angular.z = msg->twist.twist.angular.z;
}
catch(tf::LookupException& ex)
{
Modified: pkg/trunk/stacks/common_msgs/nav_msgs/msg/Odometry.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/nav_msgs/msg/Odometry.msg 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/common_msgs/nav_msgs/msg/Odometry.msg 2009-08-10 00:02:33 UTC (rev 21278)
@@ -1,3 +1,3 @@
Header header
-geometry_msgs/PoseWithCovariance pose_with_covariance
-geometry_msgs/TwistWithCovariance twist_with_covariance
+geometry_msgs/PoseWithCovariance pose
+geometry_msgs/TwistWithCovariance twist
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-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -150,8 +150,8 @@
odom_stamp_ = odom->header.stamp;
odom_time_ = Time::now();
Quaternion q;
- tf::quaternionMsgToTF(odom->pose_with_covariance.pose.orientation, q);
- odom_meas_ = Transform(q, Vector3(odom->pose_with_covariance.pose.position.x, odom->pose_with_covariance.pose.position.y, 0));
+ tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
+ odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
#warning Until the robot_pose_ekf moves to taking covariances from odometry sources instead of a residual the robot_pose_ekf will not work
Modified: pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp
===================================================================
--- pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/navigation/base_local_planner/src/trajectory_planner_ros.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -173,9 +173,9 @@
bool TrajectoryPlannerROS::stopped(){
boost::recursive_mutex::scoped_lock(odom_lock_);
- return abs(base_odom_.twist_with_covariance.twist.angular.z) <= rot_stopped_velocity_
- && abs(base_odom_.twist_with_covariance.twist.linear.x) <= trans_stopped_velocity_
- && abs(base_odom_.twist_with_covariance.twist.linear.y) <= trans_stopped_velocity_;
+ return abs(base_odom_.twist.twist.angular.z) <= rot_stopped_velocity_
+ && abs(base_odom_.twist.twist.linear.x) <= trans_stopped_velocity_
+ && abs(base_odom_.twist.twist.linear.y) <= trans_stopped_velocity_;
}
double TrajectoryPlannerROS::distance(double x1, double y1, double x2, double y2){
@@ -222,11 +222,11 @@
boost::recursive_mutex::scoped_lock(odom_lock_);
try
{
- tf::Stamped<btVector3> v_in(btVector3(msg->twist_with_covariance.twist.linear.x, msg->twist_with_covariance.twist.linear.y, 0), ros::Time(), msg->header.frame_id), v_out;
+ tf::Stamped<btVector3> v_in(btVector3(msg->twist.twist.linear.x, msg->twist.twist.linear.y, 0), ros::Time(), msg->header.frame_id), v_out;
tf_->transformVector(robot_base_frame_, ros::Time(), v_in, msg->header.frame_id, v_out);
- base_odom_.twist_with_covariance.twist.linear.x = v_in.x();
- base_odom_.twist_with_covariance.twist.linear.y = v_in.y();
- base_odom_.twist_with_covariance.twist.angular.z = msg->twist_with_covariance.twist.angular.z;
+ base_odom_.twist.twist.linear.x = v_in.x();
+ base_odom_.twist.twist.linear.y = v_in.y();
+ base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
}
catch(tf::LookupException& ex)
{
@@ -411,9 +411,9 @@
geometry_msgs::Twist global_vel;
odom_lock_.lock();
- global_vel.linear.x = base_odom_.twist_with_covariance.twist.linear.x;
- global_vel.linear.y = base_odom_.twist_with_covariance.twist.linear.y;
- global_vel.angular.z = base_odom_.twist_with_covariance.twist.angular.z;
+ global_vel.linear.x = base_odom_.twist.twist.linear.x;
+ global_vel.linear.y = base_odom_.twist.twist.linear.y;
+ global_vel.angular.z = base_odom_.twist.twist.angular.z;
odom_lock_.unlock();
tf::Stamped<tf::Pose> drive_cmds;
Modified: pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -156,13 +156,13 @@
}
public:
void update(const tf::MessageNotifier<nav_msgs::Odometry>::MessagePtr & message){
- tf::Transform txi(tf::Quaternion(message->pose_with_covariance.pose.orientation.x,
- message->pose_with_covariance.pose.orientation.y,
- message->pose_with_covariance.pose.orientation.z,
- message->pose_with_covariance.pose.orientation.w),
- tf::Point(message->pose_with_covariance.pose.position.x,
- message->pose_with_covariance.pose.position.y,
- 0.0*message->pose_with_covariance.pose.position.z )); // zero height for base_footprint
+ tf::Transform txi(tf::Quaternion(message->pose.pose.orientation.x,
+ message->pose.pose.orientation.y,
+ message->pose.pose.orientation.z,
+ message->pose.pose.orientation.w),
+ tf::Point(message->pose.pose.position.x,
+ message->pose.pose.position.y,
+ 0.0*message->pose.pose.position.z )); // zero height for base_footprint
double x = txi.getOrigin().x();
double y = txi.getOrigin().y();
Modified: pkg/trunk/stacks/simulators/stage/src/stageros.cpp
===================================================================
--- pkg/trunk/stacks/simulators/stage/src/stageros.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/simulators/stage/src/stageros.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -284,13 +284,13 @@
sim_time, mapName("base_link", r), mapName("base_footprint", r)));
// Get latest odometry data
// Translate into ROS message format and publish
- this->odomMsgs[r].pose_with_covariance.pose.position.x = this->positionmodels[r]->est_pose.x;
- this->odomMsgs[r].pose_with_covariance.pose.position.y = this->positionmodels[r]->est_pose.y;
- this->odomMsgs[r].pose_with_covariance.pose.orientation = tf::createQuaternionMsgFromYaw(this->positionmodels[r]->est_pose.a);
+ this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
+ this->odomMsgs[r].pose.pose.position.y = this->positionmodels[r]->est_pose.y;
+ this->odomMsgs[r].pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->positionmodels[r]->est_pose.a);
Stg::stg_velocity_t v = this->positionmodels[r]->GetVelocity();
- this->odomMsgs[r].twist_with_covariance.twist.linear.x = v.x;
- this->odomMsgs[r].twist_with_covariance.twist.linear.y = v.y;
- this->odomMsgs[r].twist_with_covariance.twist.angular.z = v.a;
+ this->odomMsgs[r].twist.twist.linear.x = v.x;
+ this->odomMsgs[r].twist.twist.linear.y = v.y;
+ this->odomMsgs[r].twist.twist.angular.z = v.a;
//@todo Publish stall on a separate topic when one becomes available
//this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
@@ -301,11 +301,11 @@
publish(mapName(ODOM,r),this->odomMsgs[r]);
tf::Quaternion q;
- tf::quaternionMsgToTF(odomMsgs[r].pose_with_covariance.pose.orientation, q);
+ tf::quaternionMsgToTF(odomMsgs[r].pose.pose.orientation, q);
tf::Stamped<tf::Transform> tx(
tf::Transform(
q,
- tf::Point(odomMsgs[r].pose_with_covariance.pose.position.x, odomMsgs[r].pose_with_covariance.pose.position.y, 0.0)),
+ tf::Point(odomMsgs[r].pose.pose.position.x, odomMsgs[r].pose.pose.position.y, 0.0)),
sim_time, mapName("base_footprint", r), mapName("odom", r));
this->tf.sendTransform(tx);
@@ -315,13 +315,13 @@
tf::Transform gt(tf::Quaternion(gpose.a-M_PI/2.0, 0, 0),
tf::Point(gpose.y, -gpose.x, 0.0));
- this->groundTruthMsgs[r].pose_with_covariance.pose.position.x = gt.getOrigin().x();
- this->groundTruthMsgs[r].pose_with_covariance.pose.position.y = gt.getOrigin().y();
- this->groundTruthMsgs[r].pose_with_covariance.pose.position.z = gt.getOrigin().z();
- this->groundTruthMsgs[r].pose_with_covariance.pose.orientation.x = gt.getRotation().x();
- this->groundTruthMsgs[r].pose_with_covariance.pose.orientation.y = gt.getRotation().y();
- this->groundTruthMsgs[r].pose_with_covariance.pose.orientation.z = gt.getRotation().z();
- this->groundTruthMsgs[r].pose_with_covariance.pose.orientation.w = gt.getRotation().w();
+ this->groundTruthMsgs[r].pose.pose.position.x = gt.getOrigin().x();
+ this->groundTruthMsgs[r].pose.pose.position.y = gt.getOrigin().y();
+ this->groundTruthMsgs[r].pose.pose.position.z = gt.getOrigin().z();
+ this->groundTruthMsgs[r].pose.pose.orientation.x = gt.getRotation().x();
+ this->groundTruthMsgs[r].pose.pose.orientation.y = gt.getRotation().y();
+ this->groundTruthMsgs[r].pose.pose.orientation.z = gt.getRotation().z();
+ this->groundTruthMsgs[r].pose.pose.orientation.w = gt.getRotation().w();
this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
this->groundTruthMsgs[r].header.stamp = sim_time;
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/displays/robot_base2d_pose_display.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -173,9 +173,9 @@
{
if ( last_used_message_ )
{
- if ( abs(last_used_message_->pose_with_covariance.pose.position.x - message->pose_with_covariance.pose.position.x) < position_tolerance_
- && abs(last_used_message_->pose_with_covariance.pose.position.y - message->pose_with_covariance.pose.position.y) < position_tolerance_
- && abs(tf::getYaw(last_used_message_->pose_with_covariance.pose.orientation) - tf::getYaw(message->pose_with_covariance.pose.orientation)) < angle_tolerance_ )
+ if ( abs(last_used_message_->pose.pose.position.x - message->pose.pose.position.x) < position_tolerance_
+ && abs(last_used_message_->pose.pose.position.y - message->pose.pose.position.y) < position_tolerance_
+ && abs(tf::getYaw(last_used_message_->pose.pose.orientation) - tf::getYaw(message->pose.pose.orientation)) < angle_tolerance_ )
{
return;
}
@@ -201,8 +201,8 @@
}
btQuaternion bt_q;
- tf::quaternionMsgToTF(message->pose_with_covariance.pose.orientation, bt_q);
- tf::Stamped<tf::Pose> pose( btTransform( bt_q, btVector3( message->pose_with_covariance.pose.position.x, message->pose_with_covariance.pose.position.y, 0.0f ) ),
+ tf::quaternionMsgToTF(message->pose.pose.orientation, bt_q);
+ tf::Stamped<tf::Pose> pose( btTransform( bt_q, btVector3( message->pose.pose.position.x, message->pose.pose.position.y, 0.0f ) ),
message->header.stamp, frame_id );
try
Modified: pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
===================================================================
--- pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -59,9 +59,9 @@
double x = odom->pose.position.x;
double y = odom->pose.position.y;
*/
- double x = odom->pose_with_covariance.pose.position.x;
- double y = odom->pose_with_covariance.pose.position.y;
- double th = tf::getYaw(odom->pose_with_covariance.pose.orientation);
+ double x = odom->pose.pose.position.x;
+ double y = odom->pose.pose.position.y;
+ double th = tf::getYaw(odom->pose.pose.orientation);
if (!vel_init)
{
Modified: pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -43,9 +43,9 @@
fprintf(file, "%.5f ",t.toSec());
- fprintf(file, "%0.5f %0.5f %0.5f %0.5f %0.5f %0.5f %d", bL->pose_with_covariance.pose.position.x, bL->pose_with_covariance.pose.position.y,
- tf::getYaw(bL->pose_with_covariance.pose.orientation),
- bL->twist_with_covariance.twist.linear.x, bL->twist_with_covariance.twist.linear.y, bL->twist_with_covariance.twist.angular.z,
+ fprintf(file, "%0.5f %0.5f %0.5f %0.5f %0.5f %0.5f %d", bL->pose.pose.position.x, bL->pose.pose.position.y,
+ tf::getYaw(bL->pose.pose.orientation),
+ bL->twist.twist.linear.x, bL->twist.twist.linear.y, bL->twist.twist.angular.z,
0);
//@todo TODO: Decide if we should be getting stall information from a separate channel or not now that the Odometry message doesn't contain it
//bL->stall);
Modified: pkg/trunk/util/logsetta/odom/odom_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/odom/odom_extract.cpp 2009-08-09 23:29:03 UTC (rev 21277)
+++ pkg/trunk/util/logsetta/odom/odom_extract.cpp 2009-08-10 00:02:33 UTC (rev 21278)
@@ -44,12 +44,12 @@
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f %d\n",
t.toSec(),
odom->header.stamp.toSec(),
- odom->pose_with_covariance.pose.position.x,
- odom->pose_with_covariance.pose.position.y,
- tf::getYaw(odom->pose_with_covariance.pose.orientation),
- odom->twist_with_covariance.twist.linear.x,
- odom->twist_with_covariance.twist.linear.y,
- odom->twist_with_covariance.twist.angular.z,
+ odom->pose.pose.position.x,
+ odom->pose.pose.position.y,
+ tf::getYaw(odom->pose.pose.orientation),
+ odom->twist.twist.linear.x,
+ odom->twist.twist.linear.y,
+ odom->twist.twist.angular.z,
0);
//@todo TODO: Decide whether or not its important to get stall from another topic since the odometry message doesn't contain that information anymore
//odom->stall);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
