Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21206] pkg/trunk
|
From: <tf...@us...> - 2009-08-09 02:25:09
|
Revision: 21206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21206&view=rev
Author: tfoote
Date: 2009-08-09 02:24:51 +0000 (Sun, 09 Aug 2009)
Log Message:
-----------
switching from PosewithRatesStamped to Odometry #2277
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp
pkg/trunk/demos/arm_gazebo/manifest.xml
pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
pkg/trunk/stacks/simulators/stage/src/stageros.cpp
Removed Paths:
-------------
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithRatesStamped.msg
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 02:23:53 UTC (rev 21205)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -34,7 +34,7 @@
#include <pr2_mechanism_controllers/Pose3D.h>
#include <ros/node.h>
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h>
@@ -86,7 +86,7 @@
~test_run_base() {}
- geometry_msgs::PoseWithRatesStamped ground_truth;
+ nav_msgs::Odometry ground_truth;
nav_msgs::Odometry odom;
@@ -117,7 +117,7 @@
// receive messages from 2dnav stack
- geometry_msgs::PoseWithRatesStamped ground_truth;
+ nav_msgs::Odometry ground_truth;
test_run_base tb;
@@ -172,8 +172,8 @@
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.pos.orientation);
- cout << "g:: " << tb.ground_truth.vel.linear.x << " " << tb.ground_truth.vel.linear.y << " " << tb.ground_truth.vel.angular.z << " " << tb.ground_truth.pos.position.x << " " << tb.ground_truth.pos.position.y << " " << ground_truth_angles.z << " " << tb.ground_truth.header.stamp.sec + tb.ground_truth.header.stamp.nsec/1.0e9 << std::endl;
+ 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;
// cout << delta_time.toSec() << " " << run_time << endl;
node->publish("cmd_vel",cmd);
Modified: pkg/trunk/demos/arm_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/manifest.xml 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/demos/arm_gazebo/manifest.xml 2009-08-09 02:24:51 UTC (rev 21206)
@@ -17,5 +17,5 @@
<depend package="robot_state_publisher"/>
<depend package="rospy"/>
- <depend package="std_msgs"/>
+ <depend package="nav_msgs"/>
</package>
Modified: pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py
===================================================================
--- pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/demos/arm_gazebo/scripts/grasp_preprogrammed.py 2009-08-09 02:24:51 UTC (rev 21206)
@@ -49,7 +49,7 @@
from robot_msgs.msg import *
from std_msgs.msg import *
from pr2_mechanism_controllers.msg import *
-from geometry_msgs.msg import PoseWithRatesStamped
+from nav_msgs.msg import Odometry
CMD_POS_1 = 0.0
@@ -80,7 +80,7 @@
pub_r_wrist_flex = rospy.Publisher("r_wrist_flex_controller/set_command", Float64)
pub_r_wrist_roll = rospy.Publisher("r_wrist_roll_controller/set_command", Float64)
pub_r_gripper = rospy.Publisher("r_gripper_controller/set_command", Float64)
- rospy.Subscriber("r_gripper_palm_pose_ground_truth", PoseWithRatesStamped, p3dReceived)
+ rospy.Subscriber("r_gripper_palm_pose_ground_truth", Odometry, p3dReceived)
rospy.init_node(NAME, anonymous=True)
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 02:23:53 UTC (rev 21205)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -47,7 +47,7 @@
#include "geometry_msgs/Transform.h"
#include "nav_msgs/Odometry.h"
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "nav_msgs/Odometry.h"
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
@@ -75,7 +75,7 @@
param("~base_id", base_id_, 1) ;
advertise<nav_msgs::Odometry>("localizedpose", 1);
- advertise<geometry_msgs::PoseWithRatesStamped>("base_pose_ground_truth", 1) ;
+ advertise<nav_msgs::Odometry>("base_pose_ground_truth", 1) ;
m_tfServer = new tf::TransformBroadcaster();
@@ -125,14 +125,14 @@
publish("localizedpose", m_currentPos) ;
// Build Ground Truth Message
- geometry_msgs::PoseWithRatesStamped m_pose_with_rates ;
- m_pose_with_rates.header = snapshot_.header ;
- m_pose_with_rates.pose_with_rates.pose.position.x = body.pose.translation.x ;
- m_pose_with_rates.pose_with_rates.pose.position.y = body.pose.translation.y ;
- m_pose_with_rates.pose_with_rates.pose.position.z = body.pose.translation.z ;
- m_pose_with_rates.pose_with_rates.pose.orientation = body.pose.rotation ;
+ 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 ;
- publish("base_pose_ground_truth", m_pose_with_rates) ;
+ publish("base_pose_ground_truth", m_ground_truth) ;
publish_success_count_++ ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_p3d.h 2009-08-09 02:24:51 UTC (rev 21206)
@@ -34,7 +34,7 @@
#include <ros/ros.h>
#include "boost/thread/mutex.hpp"
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
namespace gazebo
{
@@ -44,7 +44,7 @@
\brief RosP3D controller.
- This controller requires to a model as its parent. The plugin broadcasts a body's pose and rates through ROS geometry_msgs::PoseWithRatesStamped message. In the example below, the plubin broadcasts pose and rate of a body named \b body_name over ROS topic name \b body_pose_groud_truth.
+ This controller requires to a model as its parent. The plugin broadcasts a body's pose and rates through ROS nav_msgs::Odometry message. In the example below, the plubin broadcasts pose and rate of a body named \b body_name over ROS topic name \b body_pose_groud_truth.
Example Usage:
\verbatim
@@ -69,7 +69,7 @@
\brief RosP3D controller
\li Starts a ROS node if none exists.
- \li This controller simulates a 6 dof position and rate sensor, publishes geometry_msgs::PoseWithRatesStamped.msg ROS topic.
+ \li This controller simulates a 6 dof position and rate sensor, publishes nav_msgs::Odometry.msg ROS topic.
\li Example Usage:
\verbatim
<model:physical name="some_fancy_model">
@@ -122,7 +122,7 @@
private: ros::Publisher pub_;
/// \brief ros message
- private: geometry_msgs::PoseWithRatesStamped poseMsg;
+ private: nav_msgs::Odometry poseMsg;
/// \brief topic name
private: std::string topicName;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/ros_sim_iface.h 2009-08-09 02:24:51 UTC (rev 21206)
@@ -19,7 +19,7 @@
*
*/
/*
- * Desc: A dynamic controller plugin that publishes ROS PoseWithRatesStamped topic for generic simiface interface. Puts body at the specified Pose (rate) etc. Only pose implemented for now.
+ * Desc: A dynamic controller plugin that publishes ROS Odometry topic for generic simiface interface. Puts body at the specified Pose (rate) etc. Only pose implemented for now.
* Author: John Hsu
* Date: 24 Sept 2008
* SVN: $Id$
@@ -31,7 +31,7 @@
#include "boost/thread/mutex.hpp"
#include <gazebo/Controller.hh>
#include <gazebo/Param.hh>
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
namespace gazebo
{
@@ -90,8 +90,8 @@
/// \brief Finalize the controller, unadvertise topics
protected: virtual void FiniChild();
- /// \brief call back when a PoseWithRatesStamped message is published
- private: void UpdateObjectPose(const geometry_msgs::PoseWithRatesStampedConstPtr& poseMsg);
+ /// \brief call back when a Odometry message is published
+private: void UpdateObjectPose(const nav_msgs::Odometry::ConstPtr& poseMsg);
/// \brief A pointer to the parent entity
private: Entity *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_p3d.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -84,7 +84,7 @@
ROS_DEBUG("==== topic name for RosP3D ======== %s", this->topicName.c_str());
if (this->topicName != "")
- this->pub_ = this->rosnode_->advertise<geometry_msgs::PoseWithRatesStamped>(this->topicName,10);
+ this->pub_ = this->rosnode_->advertise<nav_msgs::Odometry>(this->topicName,10);
}
////////////////////////////////////////////////////////////////////////////////
@@ -149,31 +149,23 @@
// pose is given in inertial frame for Gazebo, transform to the designated frame name
- this->poseMsg.pose_with_rates.pose.position.x = pos.x;
- this->poseMsg.pose_with_rates.pose.position.y = pos.y;
- this->poseMsg.pose_with_rates.pose.position.z = pos.z;
+ 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_with_rates.pose.orientation.x = rot.x;
- this->poseMsg.pose_with_rates.pose.orientation.y = rot.y;
- this->poseMsg.pose_with_rates.pose.orientation.z = rot.z;
- this->poseMsg.pose_with_rates.pose.orientation.w = rot.u;
+ 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_with_rates.velocity.linear.x = vpos.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.velocity.linear.y = vpos.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.velocity.linear.z = vpos.z + this->GaussianKernel(0,this->gaussianNoise) ;
+ 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) ;
// pass euler anglular rates
- this->poseMsg.pose_with_rates.velocity.angular.x = veul.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.velocity.angular.y = veul.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.velocity.angular.z = veul.z + this->GaussianKernel(0,this->gaussianNoise) ;
+ 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.pose_with_rates.acceleration.linear.x = this->apos.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.acceleration.linear.y = this->apos.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.acceleration.linear.z = this->apos.z + this->GaussianKernel(0,this->gaussianNoise) ;
- // pass euler anglular rates
- this->poseMsg.pose_with_rates.acceleration.angular.x = this->aeul.x + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.acceleration.angular.y = this->aeul.y + this->GaussianKernel(0,this->gaussianNoise) ;
- this->poseMsg.pose_with_rates.acceleration.angular.z = this->aeul.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 02:23:53 UTC (rev 21205)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_sim_iface.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -25,7 +25,7 @@
Date: 24 Sept 2008
SVN info: $Id$
@htmlinclude manifest.html
- @b RosSimIface plugin reads ROS PoseWithRatesStamped messages
+ @b RosSimIface plugin reads ROS Odometry messages
*/
#include <algorithm>
@@ -123,14 +123,14 @@
////////////////////////////////////////////////////////////////////////////////
// Update the controller
-void RosSimIface::UpdateObjectPose(const geometry_msgs::PoseWithRatesStampedConstPtr& poseMsg)
+void RosSimIface::UpdateObjectPose(const nav_msgs::Odometry::ConstPtr& poseMsg)
{
this->lock.lock();
Model* model = gazebo::World::Instance()->GetModelByName(this->modelName);
- Vector3 pos(poseMsg->pose_with_rates.pose.position.x,poseMsg->pose_with_rates.pose.position.y,poseMsg->pose_with_rates.pose.position.z);
- Quatern rot(poseMsg->pose_with_rates.pose.orientation.w,poseMsg->pose_with_rates.pose.orientation.x,poseMsg->pose_with_rates.pose.orientation.y,poseMsg->pose_with_rates.pose.orientation.z);
+ 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);
Pose3d modelPose(pos,rot);
model->SetPose(modelPose);
Modified: pkg/trunk/nav/teleop_base/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base/manifest.xml 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/nav/teleop_base/manifest.xml 2009-08-09 02:24:51 UTC (rev 21206)
@@ -6,6 +6,7 @@
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
+ <depend package="nav_msgs"/>
<depend package="geometry_msgs"/>
<depend package="tf" />
<depend package="control_toolbox" />
Modified: pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/nav/teleop_base/src/ground_truth_controller.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -35,7 +35,7 @@
#include <ros/node.h>
#include <ros/time.h>
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include "control_toolbox/base_position_pid.h"
#include "tf/transform_datatypes.h"
@@ -48,7 +48,7 @@
class GroundTruthController : public ros::Node
{
public:
- geometry_msgs::PoseWithRatesStamped m_ground_truth_ ; //!< Message on which we receive ground truth info
+ nav_msgs::Odometry m_ground_truth_ ; //!< Message on which we receive ground truth info
control_toolbox::BasePositionPid base_position_pid_ ; //!< Does the PID math for controlling the robot
tf::Vector3 xyt_target_ ; //!< The ground truth pose we want to acheive
bool first_time_ ;
@@ -117,7 +117,7 @@
tf::Transform ground_truth_pose ;
- tf::poseMsgToTF(m_ground_truth_.pose_with_rates.pose, ground_truth_pose) ;
+ tf::poseMsgToTF(m_ground_truth_.pose_with_covariance.pose, ground_truth_pose) ;
//! \todo Compute yaw angle in a more stable way
double yaw,pitch,roll ;
Deleted: pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithRatesStamped.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithRatesStamped.msg 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithRatesStamped.msg 2009-08-09 02:24:51 UTC (rev 21206)
@@ -1,2 +0,0 @@
-Header header
-PoseWithRates pose_with_rates
Modified: pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp
===================================================================
--- pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-09 02:23:53 UTC (rev 21205)
+++ pkg/trunk/stacks/navigation/fake_localization/fake_localization.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -56,7 +56,7 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "base_pose_ground_truth" geometry_msgs/PoseWithRatesStamped : robot's odometric pose. Only the position information is used (velocity is ignored).
+- @b "base_pose_ground_truth" nav_msgs/Odometry : robot's odometric pose. Only the position information is used (velocity is ignored).
Publishes to (name / type):
- @b "amcl_pose" geometry_msgs/PoseWithCovarianceStamped : robot's estimated pose in the map, with covariance
@@ -73,7 +73,7 @@
#include <ros/node.h>
#include <ros/time.h>
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
@@ -103,7 +103,7 @@
m_particleCloud.header.stamp = ros::Time::now();
m_particleCloud.header.frame_id = "/map";
m_particleCloud.set_poses_size(1);
- notifier = new tf::MessageNotifier<geometry_msgs::PoseWithRatesStamped>(m_tfListener, this,
+ notifier = new tf::MessageNotifier<nav_msgs::Odometry>(m_tfListener, this,
boost::bind(&FakeOdomNode::update, this, _1),
"",//empty topic it will be manually stuffed
odom_frame_id_, 100);
@@ -133,13 +133,13 @@
private:
tf::TransformBroadcaster *m_tfServer;
tf::TransformListener *m_tfListener;
- tf::MessageNotifier<geometry_msgs::PoseWithRatesStamped>* notifier;
+ tf::MessageNotifier<nav_msgs::Odometry>* notifier;
ros::Time m_lastUpdate;
double m_maxPublishFrequency;
bool m_base_pos_received;
- geometry_msgs::PoseWithRatesStamped m_basePosMsg;
+ nav_msgs::Odometry m_basePosMsg;
geometry_msgs::PoseArray m_particleCloud;
geometry_msgs::PoseWithCovarianceStamped m_currentPos;
@@ -149,20 +149,20 @@
void basePosReceived()
{
m_basePosMsg.header.frame_id = "base_footprint"; //hack to make the notifier do what I want (changed back later)
- boost::shared_ptr<geometry_msgs::PoseWithRatesStamped> message(new geometry_msgs::PoseWithRatesStamped);
+ boost::shared_ptr<nav_msgs::Odometry> message(new nav_msgs::Odometry);
*message = m_basePosMsg;
notifier->enqueueMessage(message);
// update();
}
public:
- void update(const tf::MessageNotifier<geometry_msgs::PoseWithRatesStamped>::MessagePtr & message){
- tf::Transform txi(tf::Quaternion(message->pose_with_rates.pose.orientation.x,
- message->pose_with_rates.pose.orientation.y,
- message->pose_with_rates.pose.orientation.z,
- message->pose_with_rates.pose.orientation.w),
- tf::Point(message->pose_with_rates.pose.position.x,
- message->pose_with_rates.pose.position.y,
- 0.0*message->pose_with_rates.pose.position.z )); // zero height for base_footprint
+ 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
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 02:23:53 UTC (rev 21205)
+++ pkg/trunk/stacks/simulators/stage/src/stageros.cpp 2009-08-09 02:24:51 UTC (rev 21206)
@@ -36,7 +36,7 @@
#include "boost/thread/mutex.hpp"
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
-#include <geometry_msgs/PoseWithRatesStamped.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <roslib/Time.h>
@@ -56,7 +56,7 @@
geometry_msgs::Twist *velMsgs;
sensor_msgs::LaserScan *laserMsgs;
nav_msgs::Odometry *odomMsgs;
- geometry_msgs::PoseWithRatesStamped *groundTruthMsgs;
+ nav_msgs::Odometry *groundTruthMsgs;
roslib::Time timeMsg;
// A mutex to lock access to fields that are used in message callbacks
@@ -175,7 +175,7 @@
this->velMsgs = new geometry_msgs::Twist[numRobots];
this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
this->odomMsgs = new nav_msgs::Odometry[numRobots];
- this->groundTruthMsgs = new geometry_msgs::PoseWithRatesStamped[numRobots];
+ this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
}
@@ -208,8 +208,8 @@
}
advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10);
advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10);
- advertise<geometry_msgs::PoseWithRatesStamped>(
- mapName(BASE_POSE_GROUND_TRUTH,r), 10);
+ advertise<nav_msgs::Odometry>(
+ mapName(BASE_POSE_GROUND_TRUTH,r), 10);
subscribe(mapName(CMD_VEL,r), velMsgs[r], &StageNode::cmdvelReceived, 10);
}
advertise<roslib::Time>("/time",10);
@@ -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_rates.pose.position.x = gt.getOrigin().x();
- this->groundTruthMsgs[r].pose_with_rates.pose.position.y = gt.getOrigin().y();
- this->groundTruthMsgs[r].pose_with_rates.pose.position.z = gt.getOrigin().z();
- this->groundTruthMsgs[r].pose_with_rates.pose.orientation.x = gt.getRotation().x();
- this->groundTruthMsgs[r].pose_with_rates.pose.orientation.y = gt.getRotation().y();
- this->groundTruthMsgs[r].pose_with_rates.pose.orientation.z = gt.getRotation().z();
- this->groundTruthMsgs[r].pose_with_rates.pose.orientation.w = gt.getRotation().w();
+ 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].header.frame_id = mapName("odom", r);
this->groundTruthMsgs[r].header.stamp = sim_time;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
