Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21049] pkg/trunk
|
From: <ei...@us...> - 2009-08-07 22:13:18
|
Revision: 21049
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21049&view=rev
Author: eitanme
Date: 2009-08-07 21:22:37 +0000 (Fri, 07 Aug 2009)
Log Message:
-----------
First half of the change from deprecated_msgs::RobotBase2DOdom to nav_msgs::Odometry, I think all the c++ compiles, can't speak for functionality yet, also... the python has yet to be run... this may break some things
Modified Paths:
--------------
pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.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/manifest.xml
pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml
pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py
pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
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/stacks/common_msgs/geometry_msgs/msg/PoseWithCovariance.msg
pkg/trunk/stacks/common_msgs/nav_msgs/msg/Odometry.msg
pkg/trunk/stacks/geometry/tf/include/tf/transform_datatypes.h
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/PoseWithCovarianceStamped.msg
pkg/trunk/stacks/common_msgs/geometry_msgs/msg/TwistWithCovarianceStamped.msg
Removed Paths:
-------------
pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/Odometry.msg
Modified: pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -5,7 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="tf" />
<depend package="robot_msgs" />
- <depend package="deprecated_msgs" />
+ <depend package="nav_msgs" />
<depend package="roscpp" />
<depend package="pr2_mechanism_controllers" />
</package>
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -72,12 +72,12 @@
{
_odom_mutex.lock();
if (!_odom_active){
- _odom_begin = _odom.pos.th;
- _odom_end = _odom.pos.th;
+ _odom_begin = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
+ _odom_end = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
_odom_active = true;
}
else{
- double tmp = _odom.pos.th;
+ double tmp = tf::getYaw(_odom.pose_with_covariance.pose.orientation);
AngleOverflowCorrect(tmp, _odom_end);
_odom_end = tmp;
}
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-07 21:22:37 UTC (rev 21049)
@@ -42,7 +42,7 @@
// messages
-#include "deprecated_msgs/RobotBase2DOdom.h"
+#include "nav_msgs/Odometry.h"
#include "robot_msgs/PoseDot.h"
#include "geometry_msgs/PoseWithRatesStamped.h"
#include "mechanism_msgs/MechanismState.h"
@@ -79,7 +79,7 @@
void AngleOverflowCorrect(double& a, double ref);
// messages to receive
- deprecated_msgs::RobotBase2DOdom _odom;
+ nav_msgs::Odometry _odom;
geometry_msgs::PoseWithRatesStamped _imu;
mechanism_msgs::MechanismState _mech;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox 2009-08-07 21:22:37 UTC (rev 21049)
@@ -22,7 +22,7 @@
<table border="1">
<tr><th> Topic Name </th> <th> Message Type </th><th> Description </th></tr>
<tr><td>\ref controller::BaseController::setOdomMessage "odom"</td>
- <td><a href="http://pr.willowgarage.com/pr-docs/ros-packages/std_msgs/html/classstd__msgs_1_1RobotBase2DOdom.html">RobotBase2DOdom </a> </td>
+ <td><a href="http://pr.willowgarage.com/pr-docs/ros-packages/std_msgs/html/classstd__msgs_1_1Odometry.html">Odometry </a> </td>
<td>Odometry information from base caster and wheels. </td></tr>
</table><br>
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_kinematics.h 2009-08-07 21:22:37 UTC (rev 21049)
@@ -37,7 +37,7 @@
#include <mechanism_model/robot.h>
#include <mechanism_control/controller.h>
-#include <robot_msgs/PoseDot.h>
+#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <control_toolbox/filters.h>
@@ -248,7 +248,7 @@
* @param vel Velocity of the center of rotation
* @return Velocity at the given point
*/
- robot_msgs::PoseDot pointVel2D(const geometry_msgs::Point& pos, const robot_msgs::PoseDot& vel);
+ geometry_msgs::Twist pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel);
/*!
* \brief remembers everything about the state of the robot
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_base_controller.h 2009-08-07 21:22:37 UTC (rev 21049)
@@ -84,13 +84,13 @@
* \brief callback function for setting the desired velocity using a topic
* @param cmd_vel Velocity command of the base in m/s and rad/s
*/
- void setCommand(robot_msgs::PoseDot cmd_vel);
+ void setCommand(geometry_msgs::Twist cmd_vel);
/*!
* \brief Returns the current position command
* @return Current velocity command
*/
- robot_msgs::PoseDot getCommand();
+ geometry_msgs::Twist getCommand();
/*!
* \brief class where the robot's information is computed and stored
@@ -149,22 +149,22 @@
* \brief Input speed command vector represents the desired speed requested by the node. Note that this may differ from the
* current commanded speed due to acceleration limits imposed by the controller.
*/
- robot_msgs::PoseDot cmd_vel_t_;
+ geometry_msgs::Twist cmd_vel_t_;
/*!
* \brief speed command vector used internally to represent the current commanded speed
*/
- robot_msgs::PoseDot cmd_vel_;
+ geometry_msgs::Twist cmd_vel_;
/*!
* \brief Input speed command vector represents the desired speed requested by the node.
*/
- robot_msgs::PoseDot desired_vel_;
+ geometry_msgs::Twist desired_vel_;
/*!
* \brief velocity limits specified externally
*/
- robot_msgs::PoseDot max_vel_;
+ geometry_msgs::Twist max_vel_;
/*!
* \brief acceleration limits specified externally
@@ -264,17 +264,17 @@
/*!
* \brief interpolates velocities within a given time based on maximum accelerations
*/
- robot_msgs::PoseDot interpolateCommand(robot_msgs::PoseDot start, robot_msgs::PoseDot end, geometry_msgs::Twist max_rate, double dT);
+ geometry_msgs::Twist interpolateCommand(geometry_msgs::Twist start, geometry_msgs::Twist end, geometry_msgs::Twist max_rate, double dT);
/*!
* \brief deal with cmd_vel command from 2dnav stack
*/
- void CmdBaseVelReceived(const robot_msgs::PoseDotConstPtr& msg);
+ void CmdBaseVelReceived(const geometry_msgs::TwistConstPtr& msg);
/*!
* \brief callback message, used to remember where the base is commanded to go
*/
- robot_msgs::PoseDot base_vel_msg_;
+ geometry_msgs::Twist base_vel_msg_;
/*!
* \brief generic epsilon value that is used as a minimum or maximum allowable input value
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/pr2_odometry.h 2009-08-07 21:22:37 UTC (rev 21049)
@@ -34,11 +34,10 @@
#include <Eigen/Array>
#include <Eigen/SVD>
-#include <pr2_msgs/Odometry.h>
+#include <nav_msgs/Odometry.h>
#include <realtime_tools/realtime_publisher.h>
#include <tf/tfMessage.h>
#include <pr2_mechanism_controllers/base_kinematics.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
#include <angles/angles.h>
typedef Eigen::Matrix<float, 3, 1> OdomMatrix3x1;
@@ -118,9 +117,9 @@
/*!
* \brief Builds the odometry message and prepares it for sending
- * @param msg The pr2_msgs::Odometry into which the odometry values are placed
+ * @param msg The nav_msgs::Odometry into which the odometry values are placed
*/
- void getOdometryMessage(pr2_msgs::Odometry &msg);
+ void getOdometryMessage(nav_msgs::Odometry &msg);
/*!
* \brief Takes the current odometery information and stores it into the six double parameters
@@ -134,11 +133,11 @@
void getOdometry(double &x, double &y, double &yaw, double &vx, double &vy, double &vw);
/*!
- * \brief Takes the current odometry information and stores it into the Point and PoseDot parameters
+ * \brief Takes the current odometry information and stores it into the Point and Twist parameters
* @param odom Point into which the odometry position is placed (z is theta)
* @param odom_vel into which the odometry velocity is placed
*/
- void getOdometry(geometry_msgs::Point &odom, robot_msgs::PoseDot &odom_vel);
+ void getOdometry(geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel);
/*!
* \brief Computes the base velocity from the caster positions and wheel speeds
@@ -187,9 +186,9 @@
geometry_msgs::Point odom_;
/*!
- * \brief PoseDot that remembers the current translational velocities (vel.vx, vel.vy) and angular position (ang_vel.vz)
+ * \brief Twist that remembers the current translational velocities (vel.vx, vel.vy) and angular position (ang_vel.vz)
*/
- robot_msgs::PoseDot odom_vel_;
+ geometry_msgs::Twist odom_vel_;
/*!
* \brief The type of weighting used in findWeightMatrix
@@ -224,23 +223,12 @@
/*!
* \brief The RealtimePublisher that does the realtime publishing of the odometry
*/
- realtime_tools::RealtimePublisher <pr2_msgs::Odometry>* odometry_publisher_ ;
+ realtime_tools::RealtimePublisher <nav_msgs::Odometry>* odometry_publisher_ ;
/*!
- * \brief The RealtimePublisher that does the realtime publishing of the old style odometry
- */
- realtime_tools::RealtimePublisher <deprecated_msgs::RobotBase2DOdom>* old_odometry_publisher_ ;
-
- /*!
* \brief Publishes the transform between the odometry frame and the base frame
*/
realtime_tools::RealtimePublisher <tf::tfMessage>* transform_publisher_ ;
- /*!
- * \brief Creates an old odometry message from the new odometery message
- * @param msg The old message into which the odometry will be placed
- */
- void getOldOdometryMessage(deprecated_msgs::RobotBase2DOdom &msg);
-
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -14,7 +14,7 @@
<depend package="rospy" />
<depend package="std_msgs" />
<depend package="mechanism_msgs" />
- <depend package="deprecated_msgs" />
+ <depend package="nav_msgs" />
<depend package="pr2_msgs" />
<depend package="pr2_srvs"/>
<depend package="robot_msgs" />
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_kinematics.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -188,11 +188,11 @@
}
-robot_msgs::PoseDot BaseKinematics::pointVel2D(const geometry_msgs::Point& pos, const robot_msgs::PoseDot& vel)
+geometry_msgs::Twist BaseKinematics::pointVel2D(const geometry_msgs::Point& pos, const geometry_msgs::Twist& vel)
{
- robot_msgs::PoseDot result;
- result.vel.vx = vel.vel.vx - pos.y * vel.ang_vel.vz;
- result.vel.vy = vel.vel.vy + pos.x * vel.ang_vel.vz;
- result.vel.vz = 0;
+ geometry_msgs::Twist result;
+ result.linear.x = vel.linear.x - pos.y * vel.angular.z;
+ result.linear.y = vel.linear.y + pos.x * vel.angular.z;
+ result.angular.z = 0;
return result;
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_base_controller.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -43,17 +43,17 @@
Pr2BaseController::Pr2BaseController()
{
//init variables
- cmd_vel_.vel.vx = 0;
- cmd_vel_.vel.vy = 0;
- cmd_vel_.ang_vel.vz = 0;
+ cmd_vel_.linear.x = 0;
+ cmd_vel_.linear.y = 0;
+ cmd_vel_.angular.z = 0;
- desired_vel_.vel.vx = 0;
- desired_vel_.vel.vy = 0;
- desired_vel_.ang_vel.vz = 0;
+ desired_vel_.linear.x = 0;
+ desired_vel_.linear.y = 0;
+ desired_vel_.angular.z = 0;
- cmd_vel_t_.vel.vx = 0;
- cmd_vel_t_.vel.vy = 0;
- cmd_vel_t_.ang_vel.vz = 0;
+ cmd_vel_t_.linear.x = 0;
+ cmd_vel_t_.linear.y = 0;
+ cmd_vel_t_.angular.z = 0;
new_cmd_available_ = false;
@@ -101,9 +101,9 @@
// joy_sub_ = ros_node_.subscribe(joy_listen_topic, 1, &AntiCollisionBaseController::joyCallBack, this);
//Get params from param server
- node_.param<double> ("max_vel/vx", max_vel_.vel.vx, .5);
- node_.param<double> ("max_vel/vy", max_vel_.vel.vy, .5);
- node_.param<double> ("max_vel/omegaz", max_vel_.ang_vel.vz, 10.0); //0.5
+ node_.param<double> ("max_vel/vx", max_vel_.linear.x, .5);
+ node_.param<double> ("max_vel/vy", max_vel_.linear.y, .5);
+ node_.param<double> ("max_vel/omegaz", max_vel_.angular.z, 10.0); //0.5
node_.param<double> ("max_accel/ax", max_accel_.linear.x, .2);
node_.param<double> ("max_accel/ay", max_accel_.linear.y, .2);
node_.param<double> ("max_accel/alphaz", max_accel_.linear.z, 10.0); //0.2
@@ -122,7 +122,7 @@
node_.param<double> ("state_publish_time", state_publish_time_, 0.5);
node_.param<std::string> ("cmd_topic", cmd_topic_, "/cmd_vel");
- cmd_sub_ = node_.subscribe<robot_msgs::PoseDot>(cmd_topic_, 1, &Pr2BaseController::CmdBaseVelReceived, this);
+ cmd_sub_ = node_.subscribe<geometry_msgs::Twist>(cmd_topic_, 1, &Pr2BaseController::CmdBaseVelReceived, this);
//casters
caster_controller_.resize(base_kin_.num_casters_);
@@ -166,25 +166,25 @@
}
// Set the base velocity command
-void Pr2BaseController::setCommand(robot_msgs::PoseDot cmd_vel)
+void Pr2BaseController::setCommand(geometry_msgs::Twist cmd_vel)
{
- double vel_mag = sqrt(cmd_vel.vel.vx * cmd_vel.vel.vx + cmd_vel.vel.vy * cmd_vel.vel.vy);
- double clamped_vel_mag = filters::clamp(vel_mag, -(max_vel_.vel.vx + max_vel_.vel.vy) / 2.0, (max_vel_.vel.vx + max_vel_.vel.vy) / 2.0);
+ double vel_mag = sqrt(cmd_vel.linear.x * cmd_vel.linear.x + cmd_vel.linear.y * cmd_vel.linear.y);
+ double clamped_vel_mag = filters::clamp(vel_mag, -(max_vel_.linear.x + max_vel_.linear.y) / 2.0, (max_vel_.linear.x + max_vel_.linear.y) / 2.0);
if(vel_mag > eps_)
{
- cmd_vel_t_.vel.vx = cmd_vel.vel.vx * clamped_vel_mag / vel_mag;
- cmd_vel_t_.vel.vy = cmd_vel.vel.vy * clamped_vel_mag / vel_mag;
+ cmd_vel_t_.linear.x = cmd_vel.linear.x * clamped_vel_mag / vel_mag;
+ cmd_vel_t_.linear.y = cmd_vel.linear.y * clamped_vel_mag / vel_mag;
}
else
{
- cmd_vel_t_.vel.vx = 0.0;
- cmd_vel_t_.vel.vy = 0.0;
+ cmd_vel_t_.linear.x = 0.0;
+ cmd_vel_t_.linear.y = 0.0;
}
- cmd_vel_t_.ang_vel.vz = filters::clamp(cmd_vel.ang_vel.vz, -max_vel_.ang_vel.vz, max_vel_.ang_vel.vz);
+ cmd_vel_t_.angular.z = filters::clamp(cmd_vel.angular.z, -max_vel_.angular.z, max_vel_.angular.z);
cmd_received_timestamp_ = base_kin_.robot_state_->hw_->current_time_;
- ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.vel.vx,cmd_vel.vel.vy,cmd_vel.ang_vel.vz);
- ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.vel.vx,cmd_vel_.vel.vy,cmd_vel_.ang_vel.vz);
+ ROS_DEBUG("BaseController:: command received: %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.angular.z);
+ ROS_DEBUG("BaseController:: command current: %f %f %f", cmd_vel_.linear.x,cmd_vel_.linear.y,cmd_vel_.angular.z);
ROS_DEBUG("BaseController:: clamped vel: %f", clamped_vel_mag);
ROS_DEBUG("BaseController:: vel: %f", vel_mag);
@@ -199,27 +199,27 @@
new_cmd_available_ = true;
}
-robot_msgs::PoseDot Pr2BaseController::interpolateCommand(robot_msgs::PoseDot start, robot_msgs::PoseDot end, geometry_msgs::Twist max_rate, double dT)
+geometry_msgs::Twist Pr2BaseController::interpolateCommand(geometry_msgs::Twist start, geometry_msgs::Twist end, geometry_msgs::Twist max_rate, double dT)
{
- robot_msgs::PoseDot result;
+ geometry_msgs::Twist result;
geometry_msgs::Twist alpha;
double delta(0), max_delta(0);
- delta = end.vel.vx - start.vel.vx;
+ delta = end.linear.x - start.linear.x;
max_delta = max_rate.linear.x * dT;
if(fabs(delta) <= max_delta || max_delta < eps_)
alpha.linear.x = 1;
else
alpha.linear.x = max_delta / fabs(delta);
- delta = end.vel.vy - start.vel.vy;
+ delta = end.linear.y - start.linear.y;
max_delta = max_rate.linear.y * dT;
if(fabs(delta) <= max_delta || max_delta < eps_)
alpha.linear.y = 1;
else
alpha.linear.y = max_delta / fabs(delta);
- delta = end.ang_vel.vz - start.ang_vel.vz;
+ delta = end.angular.z - start.angular.z;
max_delta = max_rate.angular.z * dT;
if(fabs(delta) <= max_delta || max_delta < eps_)
alpha.angular.z = 1;
@@ -232,19 +232,19 @@
if(alpha.angular.z < alpha_min)
alpha_min = alpha.angular.z;
- result.vel.vx = start.vel.vx + alpha_min * (end.vel.vx - start.vel.vx);
- result.vel.vy = start.vel.vy + alpha_min * (end.vel.vy - start.vel.vy);
- result.ang_vel.vz = start.ang_vel.vz + alpha_min * (end.ang_vel.vz - start.ang_vel.vz);
+ result.linear.x = start.linear.x + alpha_min * (end.linear.x - start.linear.x);
+ result.linear.y = start.linear.y + alpha_min * (end.linear.y - start.linear.y);
+ result.angular.z = start.angular.z + alpha_min * (end.angular.z - start.angular.z);
return result;
}
-robot_msgs::PoseDot Pr2BaseController::getCommand()// Return the current velocity command
+geometry_msgs::Twist Pr2BaseController::getCommand()// Return the current velocity command
{
- robot_msgs::PoseDot cmd_vel;
+ geometry_msgs::Twist cmd_vel;
pthread_mutex_lock(&pr2_base_controller_lock_);
- cmd_vel.vel.vx = cmd_vel_.vel.vx;
- cmd_vel.vel.vy = cmd_vel_.vel.vy;
- cmd_vel.ang_vel.vz = cmd_vel_.ang_vel.vz;
+ cmd_vel.linear.x = cmd_vel_.linear.x;
+ cmd_vel.linear.y = cmd_vel_.linear.y;
+ cmd_vel.angular.z = cmd_vel_.angular.z;
pthread_mutex_unlock(&pr2_base_controller_lock_);
return cmd_vel;
}
@@ -283,9 +283,9 @@
{
if(pthread_mutex_trylock(&pr2_base_controller_lock_) == 0)
{
- desired_vel_.vel.vx = cmd_vel_t_.vel.vx;
- desired_vel_.vel.vy = cmd_vel_t_.vel.vy;
- desired_vel_.ang_vel.vz = cmd_vel_t_.ang_vel.vz;
+ desired_vel_.linear.x = cmd_vel_t_.linear.x;
+ desired_vel_.linear.y = cmd_vel_t_.linear.y;
+ desired_vel_.angular.z = cmd_vel_t_.angular.z;
new_cmd_available_ = false;
pthread_mutex_unlock(&pr2_base_controller_lock_);
}
@@ -293,9 +293,9 @@
if((current_time - cmd_received_timestamp_) > timeout_)
{
- cmd_vel_.vel.vx = 0;
- cmd_vel_.vel.vy = 0;
- cmd_vel_.ang_vel.vz = 0;
+ cmd_vel_.linear.x = 0;
+ cmd_vel_.linear.y = 0;
+ cmd_vel_.angular.z = 0;
}
else
cmd_vel_ = interpolateCommand(cmd_vel_, desired_vel_, max_accel_, dT);
@@ -323,9 +323,9 @@
}
if(state_publisher_->trylock())
{
- state_publisher_->msg_.command_vx = cmd_vel_.vel.vx;
- state_publisher_->msg_.command_vy = cmd_vel_.vel.vy;
- state_publisher_->msg_.command_vw = cmd_vel_.ang_vel.vz;
+ state_publisher_->msg_.command_vx = cmd_vel_.linear.x;
+ state_publisher_->msg_.command_vy = cmd_vel_.linear.y;
+ state_publisher_->msg_.command_vw = cmd_vel_.angular.z;
for(int i = 0; i < base_kin_.num_casters_; i++)
{
state_publisher_->msg_.joint_speed[i] = base_kin_.caster_[i].caster_speed_;
@@ -368,21 +368,21 @@
void Pr2BaseController::computeDesiredCasterSteer()
{
- robot_msgs::PoseDot result;
+ geometry_msgs::Twist result;
double steer_angle_desired(0.0), steer_angle_desired_m_pi(0.0);
double error_steer(0.0), error_steer_m_pi(0.0);
- double trans_vel = sqrt(cmd_vel_.vel.vx * cmd_vel_.vel.vx + cmd_vel_.vel.vy * cmd_vel_.vel.vy);
+ double trans_vel = sqrt(cmd_vel_.linear.x * cmd_vel_.linear.x + cmd_vel_.linear.y * cmd_vel_.linear.y);
for(int i = 0; i < base_kin_.num_casters_; i++)
{
result = base_kin_.pointVel2D(base_kin_.caster_[i].offset_, cmd_vel_);
- if(trans_vel < cmd_vel_trans_eps_ && fabs(cmd_vel_.ang_vel.vz) < cmd_vel_rot_eps_)
+ if(trans_vel < cmd_vel_trans_eps_ && fabs(cmd_vel_.angular.z) < cmd_vel_rot_eps_)
{
steer_angle_desired = base_kin_.caster_[i].steer_angle_stored_;
}
else
{
- steer_angle_desired = atan2(result.vel.vy, result.vel.vx);
+ steer_angle_desired = atan2(result.linear.y, result.linear.x);
base_kin_.caster_[i].steer_angle_stored_ = steer_angle_desired;
}
steer_angle_desired_m_pi = angles::normalize_angle(steer_angle_desired + M_PI);
@@ -409,20 +409,20 @@
void Pr2BaseController::computeDesiredWheelSpeeds()
{
- robot_msgs::PoseDot wheel_point_velocity;
- robot_msgs::PoseDot wheel_point_velocity_projected;
- robot_msgs::PoseDot wheel_caster_steer_component;
- robot_msgs::PoseDot caster_2d_velocity;
+ geometry_msgs::Twist wheel_point_velocity;
+ geometry_msgs::Twist wheel_point_velocity_projected;
+ geometry_msgs::Twist wheel_caster_steer_component;
+ geometry_msgs::Twist caster_2d_velocity;
- caster_2d_velocity.vel.vx = 0;
- caster_2d_velocity.vel.vy = 0;
- caster_2d_velocity.ang_vel.vz = 0;
+ caster_2d_velocity.linear.x = 0;
+ caster_2d_velocity.linear.y = 0;
+ caster_2d_velocity.angular.z = 0;
double steer_angle_actual = 0;
for(int i = 0; i < (int) base_kin_.num_wheels_; i++)
{
base_kin_.wheel_[i].updatePosition();
- caster_2d_velocity.ang_vel.vz = kp_wheel_steer_ * base_kin_.wheel_[i].parent_->steer_velocity_desired_;
+ caster_2d_velocity.angular.z = kp_wheel_steer_ * base_kin_.wheel_[i].parent_->steer_velocity_desired_;
steer_angle_actual = base_kin_.wheel_[i].parent_->joint_->position_;
wheel_point_velocity = base_kin_.pointVel2D(base_kin_.wheel_[i].position_, cmd_vel_);
wheel_caster_steer_component = base_kin_.pointVel2D(base_kin_.wheel_[i].offset_, caster_2d_velocity);
@@ -430,9 +430,9 @@
double costh = cos(-steer_angle_actual);
double sinth = sin(-steer_angle_actual);
- wheel_point_velocity_projected.vel.vx = costh * wheel_point_velocity.vel.vx - sinth * wheel_point_velocity.vel.vy;
- wheel_point_velocity_projected.vel.vy = sinth * wheel_point_velocity.vel.vx + costh * wheel_point_velocity.vel.vy;
- base_kin_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.vel.vx + wheel_caster_steer_component.vel.vx) / (base_kin_.wheel_radius_ * base_kin_.wheel_[i].wheel_radius_scaler_);
+ wheel_point_velocity_projected.linear.x = costh * wheel_point_velocity.linear.x - sinth * wheel_point_velocity.linear.y;
+ wheel_point_velocity_projected.linear.y = sinth * wheel_point_velocity.linear.x + costh * wheel_point_velocity.linear.y;
+ base_kin_.wheel_[i].wheel_speed_cmd_ = (wheel_point_velocity_projected.linear.x + wheel_caster_steer_component.linear.x) / (base_kin_.wheel_radius_ * base_kin_.wheel_[i].wheel_radius_scaler_);
}
}
@@ -486,7 +486,7 @@
}
}
-void Pr2BaseController::CmdBaseVelReceived(const robot_msgs::PoseDotConstPtr& msg)
+void Pr2BaseController::CmdBaseVelReceived(const geometry_msgs::TwistConstPtr& msg)
{
pthread_mutex_lock(&pr2_base_controller_lock_);
base_vel_msg_ = *msg;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/pr2_odometry.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -43,7 +43,6 @@
Pr2Odometry::Pr2Odometry()
{
odometry_publisher_ = NULL;
- old_odometry_publisher_ = NULL;
transform_publisher_ = NULL;
}
@@ -54,11 +53,6 @@
odometry_publisher_->stop();
delete odometry_publisher_;
}
- if(old_odometry_publisher_)
- {
- old_odometry_publisher_->stop();
- delete old_odometry_publisher_;
- }
if(transform_publisher_)
{
@@ -99,10 +93,7 @@
if(odometry_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete odometry_publisher_;
- odometry_publisher_ = new realtime_tools::RealtimePublisher<pr2_msgs::Odometry>("/base/" + odom_frame_, 1);
- if(old_odometry_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
- delete old_odometry_publisher_;
- old_odometry_publisher_ = new realtime_tools::RealtimePublisher<deprecated_msgs::RobotBase2DOdom>("odom", 1);
+ odometry_publisher_ = new realtime_tools::RealtimePublisher<nav_msgs::Odometry>("/base/" + odom_frame_, 1);
if(transform_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
delete transform_publisher_;
transform_publisher_ = new realtime_tools::RealtimePublisher<tf::tfMessage>("tf_message", 1);
@@ -141,9 +132,9 @@
computeBaseVelocity();
- double odom_delta_x = (odom_vel_.vel.vx * costh - odom_vel_.vel.vy * sinth) * dt;
- double odom_delta_y = (odom_vel_.vel.vx * sinth + odom_vel_.vel.vy * costh) * dt;
- double odom_delta_th = odom_vel_.ang_vel.vz * dt;
+ double odom_delta_x = (odom_vel_.linear.x * costh - odom_vel_.linear.y * sinth) * dt;
+ double odom_delta_y = (odom_vel_.linear.x * sinth + odom_vel_.linear.y * costh) * dt;
+ double odom_delta_th = odom_vel_.angular.z * dt;
odom_.x += odom_delta_x;
odom_.y += odom_delta_y;
@@ -153,47 +144,32 @@
odometer_angle_ += fabs(odom_delta_th);
}
-void Pr2Odometry::getOdometry(geometry_msgs::Point &odom, robot_msgs::PoseDot &odom_vel)
+void Pr2Odometry::getOdometry(geometry_msgs::Point &odom, geometry_msgs::Twist &odom_vel)
{
odom = odom_;
odom_vel = odom_vel_;
return;
}
-void Pr2Odometry::getOdometryMessage(pr2_msgs::Odometry &msg)
+void Pr2Odometry::getOdometryMessage(nav_msgs::Odometry &msg)
{
msg.header.frame_id = odom_frame_;
msg.header.stamp.fromSec(current_time_);
- msg.odom.position.x = odom_.x;
- msg.odom.position.y = odom_.y;
- msg.odom.position.z = 0.0;
+ 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;
tf::Quaternion quat_trans = tf::Quaternion(odom_.z, 0.0, 0.0);
- msg.odom.orientation.x = quat_trans.x();
- msg.odom.orientation.y = quat_trans.y();
- msg.odom.orientation.z = quat_trans.z();
- msg.odom.orientation.w = quat_trans.w();
+ 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.odom_vel = odom_vel_;
- msg.angle = odometer_angle_;
- msg.distance = odometer_distance_;
- msg.residual = odometry_residual_max_;
-}
+ msg.twist_with_covariance.twist = odom_vel_;
-void Pr2Odometry::getOldOdometryMessage(deprecated_msgs::RobotBase2DOdom &msg)
-{
- msg.header.frame_id = "odom";
- msg.header.stamp.fromSec(current_time_);
-
- msg.pos.x = odom_.x;
- msg.pos.y = odom_.y;
- msg.pos.th = angles::normalize_angle(odom_.z);
-
- msg.vel.x = odom_vel_.vel.vx;
- msg.vel.y = odom_vel_.vel.vy;
- msg.vel.th = odom_vel_.ang_vel.vz;
-
- msg.residual = odometry_residual_max_;
+#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
+ //msg.residual = odometry_residual_max_;
}
void Pr2Odometry::getOdometry(double &x, double &y, double &yaw, double &vx, double &vy, double &vw)
@@ -201,16 +177,16 @@
x = odom_.x;
y = odom_.y;
yaw = odom_.z;
- vx = odom_vel_.vel.vx;
- vy = odom_vel_.vel.vy;
- vw = odom_vel_.ang_vel.vz;
+ vx = odom_vel_.linear.x;
+ vy = odom_vel_.linear.y;
+ vw = odom_vel_.angular.z;
}
void Pr2Odometry::computeBaseVelocity()
{
double steer_angle, wheel_speed, costh, sinth;
- robot_msgs::PoseDot caster_local_velocity;
- robot_msgs::PoseDot wheel_local_velocity;
+ geometry_msgs::Twist caster_local_velocity;
+ geometry_msgs::Twist wheel_local_velocity;
geometry_msgs::Point wheel_position;
for(int i = 0; i < base_kin_.num_wheels_; i++)
{
@@ -235,19 +211,19 @@
odometry_residual_ = cbv_lhs_ * cbv_soln_ - cbv_rhs_;
odometry_residual_max_ = odometry_residual_.cwise().abs().maxCoeff();
- odom_vel_.vel.vx = cbv_soln_(0, 0);
- odom_vel_.vel.vy = cbv_soln_(1, 0);
- odom_vel_.ang_vel.vz = cbv_soln_(2, 0);
+ odom_vel_.linear.x = cbv_soln_(0, 0);
+ odom_vel_.linear.y = cbv_soln_(1, 0);
+ odom_vel_.angular.z = cbv_soln_(2, 0);
}
double Pr2Odometry::getCorrectedWheelSpeed(int index)
{
double wheel_speed;
- robot_msgs::PoseDot caster_local_vel;
- robot_msgs::PoseDot wheel_local_vel;
- caster_local_vel.ang_vel.vz = base_kin_.wheel_[index].parent_->joint_->velocity_;
+ geometry_msgs::Twist caster_local_vel;
+ geometry_msgs::Twist wheel_local_vel;
+ caster_local_vel.angular.z = base_kin_.wheel_[index].parent_->joint_->velocity_;
wheel_local_vel = base_kin_.pointVel2D(base_kin_.wheel_[index].offset_, caster_local_vel);
- wheel_speed = base_kin_.wheel_[index].joint_->velocity_ - wheel_local_vel.vel.vx / (base_kin_.wheel_radius_ * base_kin_.wheel_[index].wheel_radius_scaler_);
+ wheel_speed = base_kin_.wheel_[index].joint_->velocity_ - wheel_local_vel.linear.x / (base_kin_.wheel_radius_ * base_kin_.wheel_[index].wheel_radius_scaler_);
return wheel_speed;
}
@@ -340,12 +316,6 @@
odometry_publisher_->unlockAndPublish();
}
- if(old_odometry_publisher_->trylock())
- {
- getOldOdometryMessage(old_odometry_publisher_->msg_);
- old_odometry_publisher_->unlockAndPublish();
- }
-
if(transform_publisher_->trylock())
{
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -35,7 +35,7 @@
#include <pr2_mechanism_controllers/Pose3D.h>
#include <ros/node.h>
#include <robot_msgs/PoseDot.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h>
#include <iostream>
#include <fstream>
@@ -87,7 +87,7 @@
~test_run_base() {}
- deprecated_msgs::RobotBase2DOdom odom;
+ nav_msgs::Odometry odom;
void odomMsgReceived()
{
@@ -162,9 +162,9 @@
if(run_time_set && delta_time.toSec() > run_time)
break;
- odom_log_file << tb.odom.pos.x << " " << tb.odom.pos.y << " " << tb.odom.pos.th << " " << tb.odom.vel.x << " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
+ 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;
- cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.vel.x << endl << " y:" << tb.odom.vel.y << endl << " omega:" << tb.odom.vel.th << 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;
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-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -36,7 +36,7 @@
#include <ros/node.h>
#include <geometry_msgs/PoseWithRatesStamped.h>
#include <robot_msgs/PoseDot.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h>
static int done = 0;
@@ -88,7 +88,7 @@
geometry_msgs::PoseWithRatesStamped ground_truth;
- deprecated_msgs::RobotBase2DOdom odom;
+ nav_msgs::Odometry odom;
int subscriber_connected;
@@ -121,9 +121,9 @@
test_run_base tb;
- node->subscribe("base_pose_ground_truth",tb.ground_truth,&test_run_base::odomMsgReceived,&tb,10);
+ node->subscribe("base_pose_ground_truth",tb.ground_truth,&test_run_base::groundTruthMsgReceived,&tb,10);
- node->subscribe("odom",tb.odom,&test_run_base::groundTruthMsgReceived,&tb,10);
+ node->subscribe("odom",tb.odom,&test_run_base::odomMsgReceived,&tb,10);
signal(SIGINT, finalize);
signal(SIGQUIT, finalize);
@@ -174,7 +174,7 @@
// ang_rates = GetAsEuler(tb.ground_truth.vel.ang_vel);
ground_truth_angles = GetAsEuler(tb.ground_truth.pos.orientation);
cout << "g:: " << tb.ground_truth.vel.vel.vx << " " << tb.ground_truth.vel.vel.vy << " " << tb.ground_truth.vel.ang_vel.vz << " " << 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;
- cout << "o:: " << tb.odom.vel.x << " " << tb.odom.vel.y << " " << tb.odom.vel.th << " " << tb.odom.pos.x << " " << tb.odom.pos.y << " " << tb.odom.pos.th << " " << tb.odom.header.stamp.sec + tb.odom.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);
sleep_time.sleep();
Modified: pkg/trunk/demos/door_demos_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/door_demos_gazebo/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -10,4 +10,5 @@
<depend package="pr2_mechanism_controllers"/>
<depend package="rospy"/>
<depend package="robot_state_publisher"/>
+ <depend package="nav_msgs"/>
</package>
Modified: pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py
===================================================================
--- pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/door_demos_gazebo/scripts/initialize_nav.py 2009-08-07 21:22:37 UTC (rev 21049)
@@ -50,7 +50,7 @@
from robot_actions.msg import *
from nav_robot_actions.msg import *
from robot_msgs.msg import *
-from deprecated_msgs.msg import *
+from nav_msgs.msg import *
from tf.transformations import *
from numpy import *
@@ -145,20 +145,22 @@
print " " + "z: " + str(p3d.vel.ang_vel.vz)
+ def quaternionMsgToList(self, q):
+ return [q.x, q.y, q.z, q.w]
def odomInput(self, odom):
#self.printBaseOdom(odom)
# initialize odom
if self.odom_initialized == False or self.p3d_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pos.x
- self.odom_yi = odom.pos.y
- self.odom_qi = quaternion_from_euler(0,0,odom.pos.th,'rxyz')
+ 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)
else:
# update odom
- self.odom_x = odom.pos.x
- self.odom_y = odom.pos.y
- self.odom_q = quaternion_from_euler(0,0,odom.pos.th,'rxyz')
+ 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)
def p3dInput(self, p3d):
#self.printBaseP3D(p3d)
@@ -221,7 +223,7 @@
pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
pub_pose = rospy.Publisher("initialpose", PoseWithCovariance)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.Subscriber("odom" , RobotBase2DOdom , self.odomInput)
+ 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)
Modified: pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -7,6 +7,7 @@
<depend package="3dnav_pr2"/>
<depend package="planning_environment"/>
+ <depend package="nav_msgs"/>
<!--
<depend package="2dnav_pr2"/>
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-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/tabletop_manipulation/tabletop_scripts/old/scripts/approachtable.py 2009-08-07 21:22:37 UTC (rev 21049)
@@ -39,7 +39,7 @@
from robot_srvs.srv import FindTable, FindTableRequest
from robot_msgs.msg import Polygon3D, Point
from visualization_msgs.msg import Marker
-from deprecated_msgs.msg import RobotBase2DOdom
+from nav_msgs.msg import Odometry
from tf.msg import tfMessage
import bullet
import tf
@@ -59,7 +59,7 @@
self.odom_pose = None
self.robot_position = None
self.vm = None
- rospy.Subscriber('odom', RobotBase2DOdom, self.odomCallback)
+ rospy.Subscriber('odom', Odometry, self.odomCallback)
rospy.Subscriber('tf_message', tfMessage, self.tfCallback)
self.pub_vis = rospy.Publisher("visualization_marker", Marker)
self.global_frame = rospy.get_param('/global_frame_id')
@@ -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.pos.x -
+ self.robot_position = Point((self.odom_pose.pose_with_covariance.pose.position.x -
msg.transforms[0].transform.translation.x),
- (self.odom_pose.pos.y -
+ (self.odom_pose.pose_with_covariance.pose.position.y -
msg.transforms[0].transform.translation.y),
0.0)
Modified: pkg/trunk/demos/test_2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/test_2dnav_gazebo/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -19,7 +19,7 @@
<depend package="robot_msgs"/>
<depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
- <depend package="deprecated_msgs"/>
+ <depend package="nav_msgs"/>
<depend package="rviz"/>
<depend package="rxtools"/>
<depend package="2dnav_pr2"/>
Modified: pkg/trunk/demos/test_2dnav_gazebo/set_goal.py
===================================================================
--- pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/demos/test_2dnav_gazebo/set_goal.py 2009-08-07 21:22:37 UTC (rev 21049)
@@ -50,7 +50,7 @@
from nav_robot_actions.msg import MoveBaseState
from geometry_msgs.msg import Pose,Quaternion,Point, PoseWithRatesStamped, PoseStamped, PoseWithCovariance
from robot_msgs.msg import PoseDot
-from deprecated_msgs.msg import RobotBase2DOdom
+from nav_msgs.msg import Odometry
import tf.transformations as tft
from numpy import float64
@@ -147,20 +147,22 @@
print " " + "z: " + str(p3d.vel.ang_vel.vz)
+ def quaternionMsgToList(self, q):
+ return [q.x, q.y, q.z, q.w]
def odomInput(self, odom):
#self.printBaseOdom(odom)
# initialize odom
if self.odom_initialized == False or self.p3d_initialized == False:
self.odom_initialized = True
- self.odom_xi = odom.pos.x
- self.odom_yi = odom.pos.y
- self.odom_qi = tft.quaternion_from_euler(0,0,odom.pos.th,'rxyz')
+ 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)
else:
# update odom
self.odom_x = odom.pos.x
self.odom_y = odom.pos.y
- self.odom_q = tft.quaternion_from_euler(0,0,odom.pos.th,'rxyz')
+ self.odom_q = quaternionMsgToList(odom.pose_with_covariance.orientation)
def p3dInput(self, p3d):
#self.printBaseP3D(p3d)
@@ -223,7 +225,7 @@
pub_goal = rospy.Publisher("/move_base/activate", PoseStamped)
pub_pose = rospy.Publisher("initialpose", PoseWithCovariance)
rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.Subscriber("odom" , RobotBase2DOdom , self.odomInput)
+ 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)
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -5,7 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
- <depend package="deprecated_msgs" />
+ <depend package="nav_msgs" />
<depend package="robot_msgs" />
<depend package="mocap_msgs" />
<depend package="phase_space" />
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-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -46,7 +46,7 @@
#include "mocap_msgs/MocapBody.h"
#include "geometry_msgs/Transform.h"
-#include "deprecated_msgs/RobotBase2DOdom.h"
+#include "nav_msgs/Odometry.h"
#include "geometry_msgs/PoseWithRatesStamped.h"
#include <tf/tf.h>
@@ -74,7 +74,7 @@
param("~publish_transform", publish_transform_, true) ;
param("~base_id", base_id_, 1) ;
- advertise<deprecated_msgs::RobotBase2DOdom>("localizedpose", 1);
+ advertise<nav_msgs::Odometry>("localizedpose", 1);
advertise<geometry_msgs::PoseWithRatesStamped>("base_pose_ground_truth", 1) ;
m_tfServer = new tf::TransformBroadcaster();
@@ -114,12 +114,12 @@
m_currentPos.header = snapshot_.header;
m_currentPos.header.frame_id = "map";
- m_currentPos.pos.x = body.pose.translation.x;
- m_currentPos.pos.y = body.pose.translation.y;
+ m_currentPos.pose_with_covariance.pose.position.x = body.pose.translation.x;
+ m_currentPos.pose_with_covariance.pose.position.y = body.pose.translation.y;
double yaw,pitch,roll;
mytf.getBasis().getEulerZYX(yaw,pitch,roll);
- m_currentPos.pos.th = yaw;
+ m_currentPos.pose_with_covariance.pose.orientation= tf::createQuaternionMsgFromYaw(yaw);
if (publish_localized_pose_)
publish("localizedpose", m_currentPos) ;
@@ -151,7 +151,7 @@
private:
mocap_msgs::MocapSnapshot snapshot_;
- deprecated_msgs::RobotBase2DOdom m_currentPos;
+ nav_msgs::Odometry m_currentPos;
tf::TransformBroadcaster *m_tfServer;
unsigned int publish_success_count_ ;
unsigned int publish_attempt_count_ ;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -16,6 +16,7 @@
<depend package="prosilica_cam"/>
<depend package="rospy"/>
<depend package="std_msgs" />
+ <depend package="nav_msgs" />
<depend package="mechanism_msgs" />
<depend package="sensor_msgs" />
<depend package="opencv_latest" />
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/ros_diffdrive.cpp 2009-08-07 21:22:37 UTC (rev 21049)
@@ -30,7 +30,7 @@
#include <ros/ros.h>
#include "tf/transform_broadcaster.h"
#include <robot_msgs/PoseDot.h>
-#include <deprecated_msgs/RobotBase2DOdom.h>
+#include <nav_msgs/Odometry.h>
#include <boost/bind.hpp>
@@ -96,13 +96,13 @@
this->rnh_ = new ros::NodeHandle();
//this->sub_ = rnh_->subscribe<geometry_msgs::PoseDot>("/cmd_vel", 100, boost::bind(&DiffDrive::cmdVelCallBack,this,_1));
this->sub_ = rnh_->subscribe<robot_msgs::PoseDot>("/cmd_vel", 100, &DiffDrive::cmdVelCallBack,this);
- this->pub_ = rnh_->advertise<deprecated_msgs::RobotBase2DOdom>("/odom", 1);
+ this->pub_ = rnh_->advertise<nav_msgs::Odometry>("/odom", 1);
// spawn 2 threads by default, ///@todo: make this a parameter
ros::MultiThreadedSpinner s(2);
boost::thread spinner_thread( boost::bind( &ros::spin, s ) );
- deprecated_msgs::RobotBase2DOdom odom;
+ nav_msgs::Odometry odom;
tf::TransformBroadcaster tfb;
ros::Duration d; d.fromSec(0.01);
@@ -125,13 +125,12 @@
tfb.sendTransform(tmp_tf_stamped);
- odom.pos.x = this->posIface->data->pose.pos.x;
- odom.pos.y = this->posIface->data->pose.pos.y;
- odom.pos.th = this->posIface->data->pose.yaw;
- odom.vel.x = this->posIface->data->velocity.pos.x;
- odom.vel.y = this->posIface->data->velocity.pos.y;
- odom.vel.th = this->posIface->data->velocity.yaw;
- odom.stall = 0;
+ 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.header.frame_id = "odom";
Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-08-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/manifest.xml 2009-08-07 21:22:37 UTC (rev 21049)
@@ -12,7 +12,7 @@
<depend package="pr2_gazebo" />
<depend package="pr2_ogre" />
<depend package="gazebo_worlds" />
- <depend package="deprecated_msgs" />
+ <depend package="nav_msgs" />
<depend package="robot_msgs" />
<depend package="std_msgs" />
<depend package="tf" />
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-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py 2009-08-07 21:22:37 UTC (rev 21049)
@@ -46,7 +46,7 @@
import os, time
import rospy, rostest
from robot_msgs.msg import *
-from deprecated_msgs.msg import *
+from nav_msgs.msg import *
TEST_DURATION = 10.0
@@ -154,13 +154,16 @@
return self.normalize_angle(angle_diff)
def printBaseOdom(self, odom):
+ orientation = odom.pose_with_covariance.orientation
+ q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
+ q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pos.x)
- print "odom pos " + "y: " + str(odom.pos.y)
- print "odom pos " + "t: " + str(odom.pos.th)
- print "odom vel " + "x: " + str(odom.vel.x)
- print "odom vel " + "y: " + str(odom.vel.y)
- print "odom vel " + "t: " + str(odom.vel.th)
+ 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 " + "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)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -182,14 +185,17 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
+ orientation = odom.pose_with_covariance.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.pos.x
- self.odom_yi = odom.pos.y
- self.odom_ei = E(0,0,odom.pos.th)
- self.odom_x = odom.pos.x - self.odom_xi
- self.odom_y = odom.pos.y - self.odom_yi
- self.odom_e.shortest_euler_distance(self.odom_ei, E(0,0,odom.pos.th))
+ self.odom_xi = odom.pose_with_covariance.pose.position.x
+ self.odom_yi = odom.pose_with_covariance.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_e.shortest_euler_distance(self.odom_ei, q.getEuler())
def p3dInput(self, p3d):
@@ -214,7 +220,7 @@
print "LNK\n"
pub = rospy.Publisher("/cmd_vel", PoseDot)
rospy.Subscriber("/base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.Subscriber("/odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("/odom", Odometry, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
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-07 21:20:03 UTC (rev 21048)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py 2009-08-07 21:22:37 UTC (rev 21049)
@@ -46,7 +46,7 @@
import os, time
import rospy, rostest
from robot_msgs.msg import *
-from deprecated_msgs.msg import *
+from nav_msgs.msg import *
TEST_DURATION = 10.0
@@ -119,13 +119,16 @@
def printBaseOdom(self, odom):
+ orientation = odom.pose_with_covariance.orientation
+ q = Q(orientation.x, orientation.y, orientation.z, orientation.w)
+ q.normalize()
print "odom received"
- print "odom pos " + "x: " + str(odom.pos.x)
- print "odom pos " + "y: " + str(odom.pos.y)
- print "odom pos " + "t: " + str(odom.pos.th)
- print "odom vel " + "x: " + str(odom.vel.x)
- print "odom vel " + "y: " + str(odom.vel.y)
- print "odom vel " + "t: " + str(odom.vel.th)
+ 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 " + "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)
def printBaseP3D(self, p3d):
print "base pose ground truth received"
@@ -145,14 +148,17 @@
def odomInput(self, odom):
#self.printBaseOdom(odom)
+ orientation = odom.pose_with_covariance.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.pos.x
- self.odom_yi = odom.pos.y
- self.odom_ti = odom.pos.th
- self.odom_x = odom.pos.x - self.odom_xi
- self.odom_y = odom.pos.y - self.odom_yi
- self.odom_t = odom.pos.th - self.odom_ti
+ self.odom_xi = odom.pose_with_covariance.pose.position.x
+ self.odom_yi = odom.pose_with_covariance.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_t = q.getEuler().z - self.odom_ti
def p3dInput(self, p3d):
@@ -175,7 +181,7 @@
print "LNK\n"
pub = rospy.Publisher("/cmd_vel", PoseDot)
rospy.Subscriber("/base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
- rospy.Subscriber("/odom", RobotBase2DOdom, self.odomInput)
+ rospy.Subscriber("/odom", Odometry, self.odomInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_DURATION
while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
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_odom...
[truncated message content] |
