[Personalrobots-commit] SF.net SVN: personalrobots:[10622] pkg/trunk

[<tf...@us...>]

Revision: 10622
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10622&view=rev
Author:   tfoote
Date:     2009-02-05 23:58:09 +0000 (Thu, 05 Feb 2009)

Log Message:
-----------
converting BaseVel to PoseDot tests up through 2dnave stage and gazebo integration tests pass ros#447

Modified Paths:
--------------
    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/include/pr2_mechanism_controllers/base_controller.h
    pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
    pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.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/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
    pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
    pkg/trunk/drivers/robot/segway_apox/segway.cpp
    pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.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/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
    pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
    pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
    pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
    pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp
    pkg/trunk/mechanism/mechanism_control/scripts/detect.py
    pkg/trunk/nav/deadreckon/deadreckon.cpp
    pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
    pkg/trunk/nav/teleop_base/teleop_base.cpp
    pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp
    pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
    pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
    pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
    pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
    pkg/trunk/nav/wavefront_player/wavefront_player.cc
    pkg/trunk/prcore/pytf/manifest.xml
    pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
    pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml
    pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
    pkg/trunk/simulators/rosstage/rosstage.cc
    pkg/trunk/util/filters/CMakeLists.txt
    pkg/trunk/util/filters/manifest.xml

Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -53,7 +53,7 @@
       _completed(false)
   {
     // advertise the velocity commands
-    advertise<std_msgs::BaseVel>("cmd_vel",10);
+    advertise<std_msgs::PoseDot>("cmd_vel",10);
 
     // subscribe to messages
     subscribe("odom",            _odom, &odom_calib::odom_callback, 10);
@@ -134,7 +134,8 @@
 
     // wait for sensor measurements from odom and imu
     while (!(_odom_active && _imu_active)){
-      _vel.vx = 0; _vel.vy = 0;  _vel.vw = 0;
+      _vel.vel.vx = 0; _vel.vel.vy = 0; _vel.vel.vz = 0;
+      _vel.ang_vel.vx = 0; _vel.ang_vel.vy = 0; _vel.ang_vel.vz = 0;
       publish("cmd_vel", _vel);
       if (!_odom_active)
 	ROS_INFO("Waiting for wheel odometry to start");
@@ -157,14 +158,14 @@
       // still moving
       Duration duration = Time::now() - _time_begin;
       if (duration.toSec() <= _duration){
-	_vel.vx = _trans_vel;
-	_vel.vw = _rot_vel;
+	_vel.vel.vx = _trans_vel;
+	_vel.ang_vel.vz = _rot_vel;
       }
       // finished turning
       else{
 	_completed = true;
-	_vel.vx = 0;
-	_vel.vw = 0;
+	_vel.vel.vx = 0;
+	_vel.ang_vel.vz = 0;
       }
       publish("cmd_vel", _vel);
       usleep(50000);
@@ -176,8 +177,8 @@
   {
     // give robot time to stop
     for (unsigned int i=0; i<10; i++){
-      _vel.vx = 0;
-      _vel.vw = 0;
+      _vel.vel.vx = 0;
+      _vel.ang_vel.vz = 0;
       publish("cmd_vel", _vel);
       usleep(50000);
     }

Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -43,7 +43,7 @@
 
 // messages
 #include "std_msgs/RobotBase2DOdom.h"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
 #include "std_msgs/PoseWithRatesStamped.h"
 #include "robot_msgs/MechanismState.h"
 
@@ -84,7 +84,7 @@
   robot_msgs::MechanismState      _mech;
 
   // estimated robot pose message to send
-  std_msgs::BaseVel               _vel; 
+  std_msgs::PoseDot               _vel; 
 
   // service messages
   pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -54,7 +54,7 @@
 #include <newmat10/newmatap.h>
 
 #include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <pr2_msgs/Odometer.h>
 #include <pr2_msgs/Covariance2D.h>
 #include <pr2_msgs/BaseControllerState.h>
@@ -494,7 +494,7 @@
      * \brief deal with cmd_vel command from 2dnav stack
      */
     void CmdBaseVelReceived();
-    std_msgs::BaseVel baseVelMsg;
+    std_msgs::PoseDot baseVelMsg;
 
     /*!
      * \brief mutex lock for setting and getting ros messages

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/base_controller_pos.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -56,7 +56,7 @@
 #include <newmat10/newmatap.h>
 
 #include <std_msgs/RobotBase2DOdom.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <pr2_msgs/Odometer.h>
 #include <pr2_msgs/Covariance2D.h>
 
@@ -481,7 +481,7 @@
      * \brief deal with cmd_vel command from 2dnav stack
      */
     void CmdBaseVelReceived();
-    std_msgs::BaseVel baseVelMsg;
+    std_msgs::PoseDot baseVelMsg;
 
     /*!
      * \brief mutex lock for setting and getting ros messages

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -1145,7 +1145,7 @@
 void BaseControllerNode::CmdBaseVelReceived()
 {
   this->ros_lock_.lock();
-  this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+  this->setCommand(baseVelMsg.vel.vx,baseVelMsg.vel.vy,baseVelMsg.ang_vel.vz);
   this->ros_lock_.unlock();
 }
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/base_controller_pos.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -1187,7 +1187,7 @@
 void BaseControllerPosNode::CmdBaseVelReceived()
 {
   this->ros_lock_.lock();
-  this->setCommand(baseVelMsg.vx,baseVelMsg.vy,baseVelMsg.vw);
+  this->setCommand(baseVelMsg.vel.vx,baseVelMsg.vel.vy,baseVelMsg.vel.vz);
   this->ros_lock_.unlock();
 }
 

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -35,7 +35,6 @@
 #include <libTF/libTF.h>
 #include <ros/node.h>
 #include <robot_srvs/SetJointCmd.h>
-#include <std_msgs/BaseVel.h>
 #include <std_msgs/RobotBase2DOdom.h>
 
 static int done = 0;

Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -34,7 +34,7 @@
 
 #include <libTF/libTF.h>
 #include <ros/node.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/Quaternion.h>
 #include <iostream>
@@ -112,23 +112,26 @@
 
 
   /*********** Start moving the robot ************/
-  std_msgs::BaseVel cmd;
-  cmd.vx = 0;
-  cmd.vy = 0;
-  cmd.vw = 0;
+  std_msgs::PoseDot cmd;
+  cmd.vel.vx = 0;
+  cmd.vel.vy = 0;
+  cmd.vel.vz = 0;
+  cmd.ang_vel.vx = 0;
+  cmd.ang_vel.vy = 0;
+  cmd.ang_vel.vz = 0;
 
   double run_time = 0;
   bool run_time_set = false;
   int file_num = 0;
 
   if(argc >= 2)
-    cmd.vx = atof(argv[1]);
+    cmd.vel.vx = atof(argv[1]);
 
   if(argc >= 3)
-    cmd.vy = atof(argv[2]);
+    cmd.vel.vy = atof(argv[2]);
 
   if(argc >= 4)
-    cmd.vw = atof(argv[3]);
+    cmd.ang_vel.vz = atof(argv[3]);
 
   if(argc >=5)
   { 
@@ -141,7 +144,7 @@
      file_num = atoi(argv[5]);
   }
 
-  node->advertise<std_msgs::BaseVel>("cmd_vel",10);
+  node->advertise<std_msgs::PoseDot>("cmd_vel",10);
   sleep(1);
 
   libTF::Vector ang_rates;

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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_run_base_controller.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -35,7 +35,7 @@
 #include <libTF/libTF.h>
 #include <ros/node.h>
 #include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/Quaternion.h>
 
@@ -131,29 +131,33 @@
 
 
   /*********** Start moving the robot ************/
-  std_msgs::BaseVel cmd;
-  cmd.vx = 0;
-  cmd.vy = 0;
-  cmd.vw = 0;
+  std_msgs::PoseDot cmd;
+  cmd.vel.vx = 0;
+  cmd.vel.vy = 0;
+  cmd.vel.vz = 0;
+  cmd.ang_vel.vx = 0;
+  cmd.ang_vel.vy = 0;
+  cmd.ang_vel.vz = 0;
 
   double run_time = 0;
   bool run_time_set = false;
+  int file_num = 0;
 
   if(argc >= 2)
-    cmd.vx = atof(argv[1]);
+    cmd.vel.vx = atof(argv[1]);
 
   if(argc >= 3)
-    cmd.vy = atof(argv[2]);
+    cmd.vel.vy = atof(argv[2]);
 
   if(argc >= 4)
-    cmd.vw = atof(argv[3]);
+    cmd.ang_vel.vz = atof(argv[3]);
 
   if(argc ==5)
   { 
      run_time = atof(argv[4]);
      run_time_set = true;
   }
-  node->advertise<std_msgs::BaseVel>("cmd_vel",1);
+  node->advertise<std_msgs::PoseDot>("cmd_vel",1);
   sleep(1);
   node->publish("cmd_vel",cmd);
   sleep(1);
@@ -164,7 +168,7 @@
   while(!done)
   {
      ros::Duration delta_time = ros::Time::now() - start_time;
-     cout << "Sending out command " << cmd.vx << " " << cmd.vy << " " << cmd.vw  << endl;
+     cout << "Sending out command " << cmd.vel.vx << " " << cmd.vel.vy << " " << cmd.ang_vel.vz  << endl;
      if(run_time_set && delta_time.toSec() > run_time)
         break;
     //   ang_rates = GetAsEuler(tb.ground_truth.vel.ang_vel);

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -82,16 +82,9 @@
   Eigen::Matrix<float,6,5> constraint_jac_;
   Eigen::Matrix<float,6,1> constraint_wrench_;
   Eigen::Matrix<float,5,1> constraint_force_;
-  // joint constraint
-  Eigen::MatrixXf joint_constraint_force_;
-  Eigen::MatrixXf joint_constraint_jac_;
-  Eigen::MatrixXf joint_constraint_null_space_;
-  
   Eigen::MatrixXf task_jac_;
   Eigen::MatrixXf identity_;
-  Eigen::MatrixXf identity_joint_;
   Eigen::MatrixXf constraint_null_space_;
-  Eigen::MatrixXf joint_constraint_torq_;
   Eigen::MatrixXf constraint_torq_;
   Eigen::MatrixXf task_torq_;
   KDL::Frame endeffector_frame_;
@@ -99,14 +92,12 @@
 
   // some parameters to define the constraint
   double wall_x;
-  double elbow_limit;
   double threshold_x;
   double wall_r;
   double threshold_r;
   double f_x_max;
   double f_r_max;
   double f_pose_max;
-  double f_limit_max;
   double desired_roll_;
   double desired_pitch_;
   double desired_yaw_;

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -51,7 +51,6 @@
 : jnt_to_jac_solver_(NULL),
   jnt_to_pose_solver_(NULL),
   initialized_(false)
-  
 {
   constraint_jac_.setZero();
   constraint_wrench_.setZero();
@@ -100,26 +99,19 @@
   node->param("constraint/wall_x"       , wall_x      , 0.8) ; /// location of the wall
   node->param("constraint/threshold_x"  , threshold_x , 0.1 ) ; /// distance within the wall to apply constraint force
   node->param("constraint/wall_r"       , wall_r      , 0.2 ) ; /// cylinder radius
-  node->param("constraint/elbow_limit"  , elbow_limit , -0.523 ) ; /// elbow limit
   node->param("constraint/threshold_r"  , threshold_r , 0.1) ; /// radius over with constraint is applied
   node->param("constraint/f_x_max"      , f_x_max     , -1000.0) ; /// max x force
   node->param("constraint/f_r_max"      , f_r_max     , -1000.0) ; /// max r force
   node->param("constraint/f_r_max"      , f_pose_max  , 20.0) ; /// max r force
-  node->param("constraint/f_limit_max"  , f_limit_max , 20.0) ; /// max r force
-  
+
   // Constructs solvers and allocates matrices.
   unsigned int num_joints   = kdl_chain_.getNrOfJoints();
   jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
   jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
   task_jac_ = Eigen::MatrixXf::Zero(6, num_joints);
   identity_ = Eigen::MatrixXf::Identity(6, 6);
-  identity_joint_ = Eigen::MatrixXf::Identity(num_joints, num_joints);
-  constraint_null_space_ = Eigen::MatrixXf::Zero(6, 6);
-  joint_constraint_force_= Eigen::MatrixXf::Zero(1, 1);
-  joint_constraint_jac_= Eigen::MatrixXf::Zero(num_joints, 1);
-  joint_constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
+  constraint_null_space_ = Eigen::MatrixXf::Zero(num_joints, num_joints);
   constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
-  joint_constraint_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
   task_torq_ = Eigen::MatrixXf::Zero(num_joints, 1);
   task_wrench_ = Eigen::Matrix<float,6,1>::Zero();
 
@@ -176,9 +168,8 @@
 
   // convert the wrench into joint torques
 
-  joint_constraint_torq_ = joint_constraint_jac_.transpose()*joint_constraint_force_;
-  constraint_torq_ = joint_constraint_null_space_*task_jac_.transpose() * constraint_wrench_;
-  task_torq_ = joint_constraint_null_space_ * task_jac_.transpose()* constraint_null_space_ * task_wrench_;
+  constraint_torq_ = task_jac_.transpose() * constraint_wrench_;
+  task_torq_ = task_jac_.transpose()* constraint_null_space_ * task_wrench_;
 
   //ROS_ERROR("%f %f %f", task_torq_(0),task_torq_(1),task_torq_(1));
   //ROS_ERROR("%.3f %.3f %.3f",constraint_null_space_(0,0), constraint_null_space_(1,1), constraint_null_space_(2,2));
@@ -187,7 +178,7 @@
 
   JntArray jnt_eff(kdl_chain_.getNrOfJoints());
   for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); ++i)
-    jnt_eff(i) = joint_constraint_torq_(i)+constraint_torq_(i) + task_torq_(i);
+    jnt_eff(i) = constraint_torq_(i) + task_torq_(i);
   chain_.setEfforts(jnt_eff, robot_->joint_states_);
 }
 
@@ -287,22 +278,6 @@
   {
     f_yaw = 0;
   }
-  
-  //joint constraint force - stop the elbow from going past -30 degrees (.5235 rad)
-  JntArray jnt_pos(kdl_chain_.getNrOfJoints());
-  chain_.getPositions(robot_->joint_states_, jnt_pos);
-  
-  double joint_e = elbow_limit-jnt_pos(3);
-   
-  if(joint_e < 0)
-  {
-    joint_constraint_jac_(3)=1;
-    joint_constraint_force_(0)=joint_e*f_limit_max;
-  }
-  else
-  {
-    joint_constraint_force_(0)=0;
-  }
 
   constraint_force_(0) = f_x;
   constraint_force_(1) = f_r;
@@ -316,7 +291,6 @@
   // Compute generalized inverse, this is the transpose as long as the constraints are
   // orthonormal to eachother. Will replace with QR method later.
   constraint_null_space_ = identity_ - constraint_jac_*constraint_jac_.transpose();
-  joint_constraint_null_space_ = identity_joint_ - joint_constraint_jac_*joint_constraint_jac_.transpose();
 }
 
 

Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc	2009-02-05 23:58:09 UTC (rev 10622)
@@ -58,7 +58,7 @@
 @section topic ROS topics
 
 Subscribes to (name/type):
-- @b "cmd_vel"/BaseVel : velocity commands to differentially drive the robot.
+- @b "cmd_vel"/PoseDot : velocity commands to differentially drive the robot.
 
 Publishes to (name / type):
 - @b "odom"/RobotBase2DOdom : odometry data from the robot.
@@ -88,7 +88,7 @@
 // Messages that I need
 #include <std_msgs/RobotBase2DOdom.h>
 //#include <std_msgs/RobotBase2DCmdVel.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <robot_msgs/BatteryState.h>
 
 #define PLAYER_QUEUE_LEN 32
@@ -103,7 +103,7 @@
     QueuePointer q;
 
     std_msgs::RobotBase2DOdom odom;
-    std_msgs::BaseVel cmdvel;
+    std_msgs::PoseDot cmdvel;
 
     tf::TransformBroadcaster tf;
   
@@ -256,9 +256,9 @@
       //cmd.vel.px = this->cmdvel.vel.x;
       //cmd.vel.py = this->cmdvel.vel.y;
       //cmd.vel.pa = this->cmdvel.vel.th;
-      cmd.vel.px = this->cmdvel.vx;
+      cmd.vel.px = this->cmdvel.vel.vx;
       cmd.vel.py = 0.0;
-      cmd.vel.pa = this->cmdvel.vw;
+      cmd.vel.pa = this->cmdvel.ang_vel.vz;
       cmd.state = 1;
 
 

Modified: pkg/trunk/drivers/robot/segway_apox/segway.cpp
===================================================================
--- pkg/trunk/drivers/robot/segway_apox/segway.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/robot/segway_apox/segway.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -3,7 +3,7 @@
 #include <unistd.h>
 #include "ros/node.h"
 #include "boost/thread/mutex.hpp"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
 #include "std_msgs/RobotBase2DOdom.h"
 #include "std_msgs/String.h"
 #include "rmp_frame.h"
@@ -33,7 +33,7 @@
 
 		static const int max_x_stepsize = 5, max_yaw_stepsize = 2;
 
-    std_msgs::BaseVel cmd_vel;
+    std_msgs::PoseDot cmd_vel;
     std_msgs::RobotBase2DOdom odom;
     std_msgs::String op_mode;
 		rmp_frame_t rmp;
@@ -116,8 +116,8 @@
 {
   req_mutex.lock();
   req_time = ros::Time::now().toSec();
-  req_x_vel = cmd_vel.vx;
-	req_yaw_rate = cmd_vel.vw;
+  req_x_vel = cmd_vel.vel.vx;
+	req_yaw_rate = cmd_vel.ang_vel.vz;
 	req_mutex.unlock();
 }
 

Modified: pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml
===================================================================
--- pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/hztest_pr2_odom.xml	2009-02-05 23:58:09 UTC (rev 10622)
@@ -7,8 +7,8 @@
     <!-- Note how we use a different parameter namespace and node name
     for this test (odom_test vs. scan_test). -->
     <param name="topic" value="odom" />
-    <param name="hz" value="84.128" />
-    <param name="hzerror" value="50.0" />
+    <param name="hz" value="10.0" />
+    <param name="hzerror" value="1.0" />
     <param name="test_duration" value="2.0" />
     <param name="check_intervals" value="false" />
   </test>

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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomw_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -210,13 +210,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 10.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+            pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
             # display what odom thinks
             #print " odom    " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomx_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 10.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+            pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
             # display what odom thinks
             #print " odom    " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_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_odomxy_gt.py	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxy_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 10.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+            pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
             # display what odom thinks
             #print " odom    " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)

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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomxyw_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -210,13 +210,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 10.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+            pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
             # display what odom thinks
             #print " odom    " + " x: " + str(self.odom_e.x) + " y: " + str(self.odom_e.y) + " t: " + str(self.odom_e.t)

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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_odomy_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -171,13 +171,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 10.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(TARGET_VX,TARGET_VY,TARGET_VW))
+            pub.publish(PoseDot(Velocity(TARGET_VX,TARGET_VY, 0), AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
             # display what odom thinks
             #print " odom    " + " x: " + str(self.odom_x) + " y: " + str(self.odom_y) + " t: " + str(self.odom_t)

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-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/drivers/simulator/test_pr2_mechanism_controllers_gazebo/test_base_vw_gt.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -116,13 +116,13 @@
     
     def test_base(self):
         print "LNK\n"
-        pub = rospy.Publisher("cmd_vel", BaseVel)
+        pub = rospy.Publisher("cmd_vel", PoseDot)
         rospy.Subscriber("base_pose_ground_truth", PoseWithRatesStamped, self.p3dInput)
         rospy.Subscriber("odom",                   RobotBase2DOdom,      self.odomInput)
         rospy.init_node(NAME, anonymous=True)
         timeout_t = time.time() + 60.0
         while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
-            pub.publish(BaseVel(0.0,0.0,TARGET_VW))
+            pub.publish(PoseDot(Velocity(0.0,0.0,0),AngularVelocity(0,0,TARGET_VW)))
             time.sleep(0.1)
         self.assert_(self.success)
         

Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -280,7 +280,7 @@
   {
     // receive data
     boost::mutex::scoped_lock lock(vel_mutex_);
-    vel_desi_(1) = vel_.vx;   vel_desi_(2) = vel_.vw;
+    vel_desi_(1) = vel_.vel.vx;   vel_desi_(2) = vel_.ang_vel.vz;
 
     // active
     //if (!vel_active_) vel_active_ = true;

Modified: pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h
===================================================================
--- pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/estimation/robot_pose_ekf/src/odom_estimation_node.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -44,7 +44,7 @@
 
 // messages
 #include "std_msgs/RobotBase2DOdom.h"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
 #include "std_msgs/PoseWithRatesStamped.h"
 #include "std_msgs/PoseStamped.h"
 #include "robot_msgs/VOPose.h"
@@ -90,7 +90,7 @@
   OdomEstimation my_filter_;
 
   // messages to receive
-  std_msgs::BaseVel               vel_;  
+  std_msgs::PoseDot               vel_;  
   std_msgs::RobotBase2DOdom       odom_;  
   std_msgs::PoseWithRatesStamped  imu_;  
   robot_msgs::VOPose              vo_;  

Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh	2009-02-05 23:58:09 UTC (rev 10622)
@@ -54,8 +54,8 @@
 // Message structures used
 #include <robot_msgs/Planner2DState.h>
 #include <robot_msgs/Planner2DGoal.h>
+#include <std_msgs/PoseDot.h>
 #include <std_msgs/LaserScan.h>
-#include <std_msgs/BaseVel.h>
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/PointCloud.h>
 

Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -34,7 +34,7 @@
 
 
 #include <highlevel_controllers/move_base.hh>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <std_msgs/PointCloud.h>
 #include <std_msgs/Pose2DFloat32.h>
 #include <std_msgs/Polyline2D.h>
@@ -277,7 +277,7 @@
       advertise<std_msgs::Polyline2D>("robot_footprint", 1);
 
       // Advertize message to publish velocity cmds
-      advertise<std_msgs::BaseVel>("cmd_vel", 1);
+      advertise<std_msgs::PoseDot>("cmd_vel", 1);
 
       //Advertize message to publish local goal for head to track
       advertise<std_msgs::PointStamped>("head_controller/head_track_point", 1);
@@ -660,7 +660,7 @@
       // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead. 
       // We want to be able to stop the robot in that case
       bool planOk = checkWatchDog() && isValid();
-      std_msgs::BaseVel cmdVel; // Commanded velocities      
+      std_msgs::PoseDot cmdVel; // Commanded velocities      
 
       // if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
       // orientation
@@ -668,10 +668,10 @@
         double uselessPitch, uselessRoll, yaw;
         global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
         ROS_DEBUG("Moving to desired goal orientation\n");
-        cmdVel.vx = 0;
-        cmdVel.vy = 0;
-        cmdVel.vw =  stateMsg.goal.th - yaw;
-        cmdVel.vw = cmdVel.vw >= 0.0 ? cmdVel.vw + .4 : cmdVel.vw - .4;
+        cmdVel.vel.vx = 0;
+        cmdVel.vel.vy = 0;
+        cmdVel.ang_vel.vz =  stateMsg.goal.th - yaw;
+        cmdVel.ang_vel.vz = cmdVel.ang_vel.vz >= 0.0 ? cmdVel.ang_vel.vz + .4 : cmdVel.ang_vel.vz - .4;
       }
       else {
         // Refine the plan to reflect progress made. If no part of the plan is in the local cost window then
@@ -697,10 +697,10 @@
         }
 
         // Set current velocities from odometry
-        std_msgs::BaseVel currentVel;
-        currentVel.vx = base_odom_.vel.x;
-        currentVel.vy = base_odom_.vel.y;
-        currentVel.vw = base_odom_.vel.th;
+        std_msgs::PoseDot currentVel;
+        currentVel.vel.vx = base_odom_.vel.x;
+        currentVel.vel.vy = base_odom_.vel.y;
+        currentVel.ang_vel.vz = base_odom_.vel.th;
 
 	ros::Time start = ros::Time::now();
         // Create a window onto the global cost map for the velocity controller
@@ -723,9 +723,9 @@
 
         if(!planOk){
           // Zero out the velocities
-          cmdVel.vx = 0;
-          cmdVel.vy = 0;
-          cmdVel.vw = 0;
+          cmdVel.vel.vx = 0;
+          cmdVel.vel.vy = 0;
+          cmdVel.ang_vel.vz = 0;
         }
         else {
           publishPath(false, localPlan);
@@ -885,10 +885,10 @@
 
     void MoveBase::stopRobot(){
       ROS_DEBUG("Stopping the robot now!\n");
-      std_msgs::BaseVel cmdVel; // Commanded velocities
-      cmdVel.vx = 0.0;
-      cmdVel.vy = 0.0;
-      cmdVel.vw = 0.0;
+      std_msgs::PoseDot cmdVel; // Commanded velocities
+      cmdVel.vel.vx = 0.0;
+      cmdVel.vel.vy = 0.0;
+      cmdVel.ang_vel.vz = 0.0;
       publish("cmd_vel", cmdVel);
     }
 

Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base_follow.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -98,7 +98,7 @@
       // case is important since we can end up with an active controller that becomes invalid through the planner looking ahead. 
       // We want to be able to stop the robot in that case
       bool planOk = checkWatchDog() && isValid();
-      std_msgs::BaseVel cmdVel; // Commanded velocities      
+      std_msgs::PoseDot cmdVel; // Commanded velocities      
 
       // if we have achieved all our waypoints but have yet to achieve the goal, then we know that we wish to accomplish our desired
       // orientation
@@ -106,10 +106,10 @@
         double uselessPitch, uselessRoll, yaw;
         global_pose_.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
         ROS_DEBUG("Moving to desired goal orientation\n");
-        cmdVel.vx = 0;
-        cmdVel.vy = 0;
-        cmdVel.vw =  stateMsg.goal.th - yaw;
-        cmdVel.vw = cmdVel.vw >= 0.0 ? cmdVel.vw + .4 : cmdVel.vw - .4;
+        cmdVel.vel.vx = 0;
+        cmdVel.vel.vy = 0;
+        cmdVel.ang_vel.vz =  stateMsg.goal.th - yaw;
+        cmdVel.ang_vel.vz = cmdVel.ang_vel.vz >= 0.0 ? cmdVel.ang_vel.vz + .4 : cmdVel.ang_vel.vz - .4;
       }
       else {
         // The plan is bogus if it is empty
@@ -119,10 +119,10 @@
         }
 
         // Set current velocities from odometry
-        std_msgs::BaseVel currentVel;
-        currentVel.vx = base_odom_.vel.x;
-        currentVel.vy = base_odom_.vel.y;
-        currentVel.vw = base_odom_.vel.th;
+        std_msgs::PoseDot currentVel;
+        currentVel.vel.vx = base_odom_.vel.x;
+        currentVel.vel.vy = base_odom_.vel.y;
+        currentVel.ang_vel.vz = base_odom_.vel.th;
 
         ros::Time start = ros::Time::now();
         // Create a window onto the global cost map for the velocity controller
@@ -145,9 +145,9 @@
 
         if(!planOk){
           // Zero out the velocities
-          cmdVel.vx = 0;
-          cmdVel.vy = 0;
-          cmdVel.vw = 0;
+          cmdVel.vel.vx = 0;
+          cmdVel.vel.vy = 0;
+          cmdVel.ang_vel.vz = 0;
         }
         else {
           publishPath(false, localPlan);

Modified: pkg/trunk/mechanism/mechanism_control/scripts/detect.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/detect.py	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/mechanism/mechanism_control/scripts/detect.py	2009-02-05 23:58:09 UTC (rev 10622)
@@ -38,7 +38,7 @@
    
 
 def listener_with_user_data():
-    rospy.Subscriber("/mechanism_state", MechanismState, callback)
+    rospy.TopicSub("/mechanism_state", MechanismState, callback)
     rospy.init_node(NAME, anonymous=True)
     rospy.spin()
 

Modified: pkg/trunk/nav/deadreckon/deadreckon.cpp
===================================================================
--- pkg/trunk/nav/deadreckon/deadreckon.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/deadreckon/deadreckon.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -29,7 +29,7 @@
 
 #include <cmath>
 #include "ros/node.h"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
 #include "std_msgs/RobotBase2DOdom.h"
 #include "deadreckon/DriveDeadReckon.h"
 using namespace std;
@@ -38,7 +38,7 @@
 class DeadReckon : public ros::Node
 {
 public:
-  std_msgs::BaseVel velMsg;
+  std_msgs::PoseDot velMsg;
   std_msgs::RobotBase2DOdom odomMsg;
   double maxTV, maxRV, distEps, headEps, finalEps, tgtX, tgtY, tgtTh;
   enum
@@ -59,7 +59,7 @@
     param("drFinalEps", finalEps, 0.05);
     advertiseService("DriveDeadReckon", &DeadReckon::dr_cb);
     subscribe("odom", odomMsg, &DeadReckon::odom_cb, 1);
-    advertise<std_msgs::BaseVel>("cmd_vel", 1);
+    advertise<std_msgs::PoseDot>("cmd_vel", 1);
   }
   bool dr_cb(deadreckon::DriveDeadReckon::Request  &req,
              deadreckon::DriveDeadReckon::Response &res)
@@ -93,7 +93,7 @@
   }
   void odom_cb()
   {
-    velMsg.vx = velMsg.vw = 0;
+    velMsg.vel.vx = velMsg.ang_vel.vz = 0;
     double dx = tgtX - odomMsg.pos.x, dy = tgtY - odomMsg.pos.y;
     double distToGo = sqrt(dx*dx + dy*dy);
     double bearing = normalizeAngle(atan2(dy, dx) - odomMsg.pos.th);
@@ -107,8 +107,8 @@
     switch(drState)
     {
       case DR_FACE_TGT:
-        velMsg.vx = 0;
-        velMsg.vw = saturate(bearing, -maxRV, maxRV);
+        velMsg.vel.vx = 0;
+        velMsg.ang_vel.vz = saturate(bearing, -maxRV, maxRV);
         if (fabs(bearing) < headEps)
         {
           drState = DR_DRIVE_TO_TGT;
@@ -116,15 +116,15 @@
         }
         break;
       case DR_DRIVE_TO_TGT:
-        velMsg.vw = 0;
+        velMsg.ang_vel.vz = 0;
         if (!backup)
         {
-          velMsg.vx = saturate(distToGo, 0.2, maxTV);
+          velMsg.vel.vx = saturate(distToGo, 0.2, maxTV);
           if (distToGo > 0.3)
-            velMsg.vw = saturate(bearing, -maxRV, maxRV);
+            velMsg.ang_vel.vz = saturate(bearing, -maxRV, maxRV);
         }
         else
-          velMsg.vx = saturate(-distToGo, -0.2, -maxTV);
+          velMsg.vel.vx = saturate(-distToGo, -0.2, -maxTV);
         if (distToGo < distEps)
         {
           drState = DR_FACE_FINAL_HEADING;
@@ -132,8 +132,8 @@
         }
         break;
       case DR_FACE_FINAL_HEADING:
-        velMsg.vx = 0;
-        velMsg.vw = saturate(finalBearing, -maxRV, maxRV);
+        velMsg.vel.vx = 0;
+        velMsg.ang_vel.vz = saturate(finalBearing, -maxRV, maxRV);
         if (fabs(finalBearing) < finalEps)
         {
           printf("going back to idle\n");
@@ -141,7 +141,7 @@
         }
         break;
       case DR_IDLE:
-        velMsg.vx = velMsg.vw = 0;
+        velMsg.vel.vx = velMsg.ang_vel.vz = 0;
     }
     publish("cmd_vel", velMsg);
   }

Modified: pkg/trunk/nav/fake_localization/ground_truth_controller.cpp
===================================================================
--- pkg/trunk/nav/fake_localization/ground_truth_controller.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/fake_localization/ground_truth_controller.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -36,7 +36,7 @@
 #include <ros/node.h>
 #include <ros/time.h>
 #include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include "control_toolbox/base_position_pid.h"
 #include "tf/transform_datatypes.h"
 
@@ -73,7 +73,7 @@
     subscribe("base_pose_ground_truth", m_ground_truth_, &GroundTruthController::updateControl, 1) ;
     subscribe("~set_cmd", cmd_, &GroundTruthController::setCommandCallback, this, 10) ;
 
-    advertise<std_msgs::BaseVel>("cmd_vel", 1) ;
+    advertise<std_msgs::PoseDot>("cmd_vel", 1) ;
     
     // Initialize the BasePositionPid util via xml
     TiXmlDocument xml ;
@@ -130,10 +130,10 @@
     tf::Vector3 vel_cmd ;
     vel_cmd = base_position_pid_.updateControl(xyt_target_, xyt_current, time_elapsed.toSec()) ;
 
-    std_msgs::BaseVel base_vel ;
-    base_vel.vx = vel_cmd.x() ;
-    base_vel.vy = vel_cmd.y() ;
-    base_vel.vw = vel_cmd.z() ;
+    std_msgs::PoseDot base_vel ;
+    base_vel.vel.vx = vel_cmd.x() ;
+    base_vel.vel.vy = vel_cmd.y() ;
+    base_vel.ang_vel.vz = vel_cmd.z() ;
     
     publish("cmd_vel", base_vel) ;
     

Modified: pkg/trunk/nav/teleop_base/teleop_base.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/teleop_base.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/teleop_base/teleop_base.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -3,14 +3,14 @@
 #include <math.h>
 #include "ros/node.h"
 #include "joy/Joy.h"
-#include "std_msgs/BaseVel.h"
+#include "std_msgs/PoseDot.h"
 
 using namespace ros;
 
 class TeleopBase : public Node
 {
    public:
-      std_msgs::BaseVel cmd, cmd_passthrough;
+      std_msgs::PoseDot cmd, cmd_passthrough;
       joy::Joy joy;
       double req_vx, req_vy, req_vw, max_vx, max_vy, max_vw;
       int axis_vx, axis_vy, axis_vw;
@@ -19,7 +19,7 @@
 
   TeleopBase(bool deadman_no_publish = false) : Node("teleop_base"), max_vx(0.6), max_vy(0.6), max_vw(0.3), deadman_no_publish_(deadman_no_publish)
       {
-         cmd.vx = cmd.vy = cmd.vw = 0;
+         cmd.vel.vx = cmd.vel.vy = cmd.ang_vel.vz = 0;
          if (!hasParam("max_vx") || !getParam("max_vx", max_vx))
             ROS_WARN("maximum linear velocity (max_vx) not set. Assuming 0.6");
          if (!hasParam("max_vy") || !getParam("max_vy", max_vy))
@@ -41,7 +41,7 @@
          printf("deadman_button: %d\n", deadman_button);
          printf("passthrough_button: %d\n", passthrough_button);
 
-         advertise<std_msgs::BaseVel>("cmd_vel", 1);
+         advertise<std_msgs::PoseDot>("cmd_vel", 1);
          subscribe("joy", joy, &TeleopBase::joy_cb, 1);
          subscribe("cmd_passthrough", cmd_passthrough, &TeleopBase::passthrough_cb, 1);
          printf("done with ctor\n");
@@ -92,16 +92,16 @@
             else
             {
                // use commands from the local sticks
-               cmd.vx = req_vx;
-               cmd.vy = req_vy;
-               cmd.vw = req_vw;
+               cmd.vel.vx = req_vx;
+               cmd.vel.vy = req_vy;
+               cmd.ang_vel.vz = req_vw;
             }
-         fprintf(stderr,"teleop_base:: %f, %f, %f\n",cmd.vx,cmd.vy,cmd.vw);
+         fprintf(stderr,"teleop_base:: %f, %f, %f\n",cmd.vel.vx,cmd.vel.vy,cmd.ang_vel.vz);
          publish("cmd_vel", cmd);
          }
          else
          {
-           cmd.vx = cmd.vy = cmd.vw = 0;
+           cmd.vel.vx = cmd.vel.vy = cmd.ang_vel.vz = 0;
             if (!deadman_no_publish_)
            {
              publish("cmd_vel", cmd);//Only publish if deadman_no_publish is enabled

Modified: pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp
===================================================================
--- pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/teleop_base_keyboard/teleop_base_keyboard.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -55,7 +55,7 @@
 - None
 
 Publishes to (name / type):
-- @b "cmd_vel"/BaseVel : velocity to the robot; sent on every keypress.
+- @b "cmd_vel"/PoseDot : velocity to the robot; sent on every keypress.
 
 <hr>
 
@@ -71,7 +71,7 @@
 #include <stdlib.h>
 
 #include <ros/node.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 
 #define KEYCODE_I 0x69
 #define KEYCODE_J 0x6a
@@ -106,18 +106,18 @@
 class TBK_Node : public ros::Node
 {
   private:
-    std_msgs::BaseVel cmdvel;
+    std_msgs::PoseDot cmdvel;
 
   public:
     TBK_Node() : ros::Node("tbk")
     {
-      advertise<std_msgs::BaseVel>("cmd_vel",1);
+      advertise<std_msgs::PoseDot>("cmd_vel",1);
     }
     ~TBK_Node() { }
     void keyboardLoop();
     void stopRobot()
     {
-      cmdvel.vx = cmdvel.vw = 0.0;
+      cmdvel.vel.vx = cmdvel.ang_vel.vz = 0.0;
       publish("cmd_vel", cmdvel);
     }
 };
@@ -277,8 +277,8 @@
     }
     if (dirty == true)
     {
-      cmdvel.vx = speed * max_tv;
-      cmdvel.vw = turn * max_rv;
+      cmdvel.vel.vx = speed * max_tv;
+      cmdvel.ang_vel.vz = turn * max_rv;
 
       publish("cmd_vel",cmdvel);
     }

Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/governor_node.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -40,7 +40,7 @@
 #include <vector>
 
 //The messages that we'll use
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <trajectory_rollout/ScoreMap2D.h>
 #include <trajectory_rollout/WavefrontPlan.h>
 #include <std_msgs/RobotBase2DOdom.h>
@@ -162,7 +162,7 @@
 
       //outgoing messages
       std_msgs::Polyline2D poly_line_msg_;
-      std_msgs::BaseVel cmd_vel_msg_;
+      std_msgs::PoseDot cmd_vel_msg_;
 
       //since both odomReceived and processPlan access robot_vel we need to lock
       boost::mutex vel_lock;

Modified: pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h
===================================================================
--- pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/trajectory_rollout/include/trajectory_rollout/trajectory_controller_ros.h	2009-02-05 23:58:09 UTC (rev 10622)
@@ -48,7 +48,7 @@
 
 #include <std_msgs/Point2DFloat32.h>
 #include <std_msgs/Position2DInt.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 
 namespace trajectory_rollout {
   /**
@@ -95,8 +95,8 @@
        */
       bool computeVelocityCommands(const std::list<std_msgs::Pose2DFloat32>& global_plan, 
           const tf::Stamped<tf::Pose>& global_pose, 
-          const std_msgs::BaseVel& global_vel, 
-          std_msgs::BaseVel& cmd_vel,
+          const std_msgs::PoseDot& global_vel, 
+          std_msgs::PoseDot& cmd_vel,
           std::list<std_msgs::Pose2DFloat32>& localPlan,
           const std::vector<costmap_2d::Observation>& observations = std::vector<costmap_2d::Observation>(0));
 

Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -57,7 +57,7 @@
     //so we can draw the robot footprint to help with debugging
     advertise<std_msgs::Polyline2D>("robot_footprint", 10);
 
-    advertise<std_msgs::BaseVel>("cmd_vel", 1);
+    advertise<std_msgs::PoseDot>("cmd_vel", 1);
     subscribe("wavefront_plan", plan_msg_, &GovernorNode::planReceived, 1);
     subscribe("odom", odom_msg_, &GovernorNode::odomReceived, 1);
   }
@@ -192,15 +192,15 @@
     }
 
     //drive the robot!
-    cmd_vel_msg_.vx = drive_cmds.getOrigin().getX();
-    cmd_vel_msg_.vy = drive_cmds.getOrigin().getY();
+    cmd_vel_msg_.vel.vx = drive_cmds.getOrigin().getX();
+    cmd_vel_msg_.vel.vy = drive_cmds.getOrigin().getY();
     drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
-    cmd_vel_msg_.vw = yaw;
+    cmd_vel_msg_.ang_vel.vz = yaw;
 
 
     if(path.cost_ < 0)
       printf("Local Plan Failed :(\n");
-    printf("Vel CMD - vx: %.2f, vy: %.2f, vt: %.2f\n", cmd_vel_msg_.vx, cmd_vel_msg_.vy, cmd_vel_msg_.vw);
+    printf("Vel CMD - vx: %.2f, vy: %.2f, vt: %.2f\n", cmd_vel_msg_.vel.vx, cmd_vel_msg_.vel.vy, cmd_vel_msg_.ang_vel.vz);
     publish("cmd_vel", cmd_vel_msg_);
   }
 

Modified: pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/trajectory_rollout/src/trajectory_controller_ros.cpp	2009-02-05 23:58:09 UTC (rev 10622)
@@ -106,8 +106,8 @@
 
   bool TrajectoryControllerROS::computeVelocityCommands(const std::list<std_msgs::Pose2DFloat32>& global_plan, 
       const tf::Stamped<tf::Pose>& global_pose, 
-      const std_msgs::BaseVel& global_vel, 
-      std_msgs::BaseVel& cmd_vel,
+      const std_msgs::PoseDot& global_vel, 
+      std_msgs::PoseDot& cmd_vel,
       std::list<std_msgs::Pose2DFloat32>& localPlan,
       const std::vector<costmap_2d::Observation>& observations){
 
@@ -118,8 +118,8 @@
     drive_cmds.frame_id_ = "base_link";
 
     tf::Stamped<tf::Pose> robot_vel;
-    btQuaternion qt(global_vel.vw, 0, 0);
-    robot_vel.setData(btTransform(qt, btVector3(global_vel.vx, global_vel.vy, 0)));
+    btQuaternion qt(global_vel.ang_vel.vz, 0, 0);
+    robot_vel.setData(btTransform(qt, btVector3(global_vel.vel.vx, global_vel.vel.vy, 0)));
     robot_vel.frame_id_ = "base_link";
     robot_vel.stamp_ = ros::Time();
 
@@ -156,12 +156,12 @@
     */
 
     //pass along drive commands
-    cmd_vel.vx = drive_cmds.getOrigin().getX();
-    cmd_vel.vy = drive_cmds.getOrigin().getY();
+    cmd_vel.vel.vx = drive_cmds.getOrigin().getX();
+    cmd_vel.vel.vy = drive_cmds.getOrigin().getY();
     double uselessPitch, uselessRoll, yaw;
     drive_cmds.getBasis().getEulerZYX(yaw, uselessPitch, uselessRoll);
 
-    cmd_vel.vw = yaw;
+    cmd_vel.ang_vel.vz = yaw;
 
     //if we cannot move... tell someone
     if(path.cost_ < 0)

Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc	2009-02-05 23:58:09 UTC (rev 10622)
@@ -77,7 +77,7 @@
 - @b "scan"/LaserScan : laser scans.  Used to temporarily modify the map for dynamic obstacles.
 
 Publishes to (name / type):
-- @b "cmd_vel"/BaseVel : velocity commands to robot
+- @b "cmd_vel"/PoseDot : velocity commands to robot
 - @b "state"/Planner2DState : current planner state (e.g., goal reached, no
 path)
 - @b "gui_path"/Polyline2D : current global path (for visualization)
@@ -116,7 +116,7 @@
 // The messages that we'll use
 #include <robot_msgs/Planner2DState.h>
 #include <robot_msgs/Planner2DGoal.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <std_msgs/PointCloud.h>
 #include <std_msgs/LaserScan.h>
 #include <std_srvs/StaticMap.h>
@@ -368,7 +368,7 @@
   advertise<robot_msgs::Planner2DState>("state",1);
   advertise<std_msgs::Polyline2D>("gui_path",1);
   advertise<std_msgs::Polyline2D>("gui_laser",1);
-  advertise<std_msgs::BaseVel>("cmd_vel",1);
+  advertise<std_msgs::PoseDot>("cmd_vel",1);
   subscribe("goal", goalMsg, &WavefrontNode::goalReceived,1);
 
   scan_notifier = new tf::MessageNotifier<std_msgs::LaserScan>(&tf, this, boost::bind(&WavefrontNode::laserReceived, this, _1), "scan", "map", 1);
@@ -548,15 +548,15 @@
 
 // Declare this globally, so that it never gets desctructed (message
 // desctruction causes master disconnect)
-std_msgs::BaseVel* cmdvel;
+std_msgs::PoseDot* cmdvel;
 
 void
 WavefrontNode::sendVelCmd(double vx, double vy, double vth)
 {
   if(!cmdvel)
-    cmdvel = new std_msgs::BaseVel();
-  cmdvel->vx = vx;
-  cmdvel->vw = vth;
+    cmdvel = new std_msgs::PoseDot();
+  cmdvel->vel.vx = vx;
+  cmdvel->ang_vel.vz = vth;
   this->ros::Node::publish("cmd_vel", *cmdvel);
   if(vx || vy || vth)
     this->stopped = false;

Modified: pkg/trunk/prcore/pytf/manifest.xml
===================================================================
--- pkg/trunk/prcore/pytf/manifest.xml	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/prcore/pytf/manifest.xml	2009-02-05 23:58:09 UTC (rev 10622)
@@ -9,6 +9,7 @@
 <review status="experimental" notes=""/>
 <url>http://pr.willowgarage.com</url>
 <depend package="rospy"/>
+<depend package="std_msgs"/>
 <depend package="tf"/>
 <sysdepend os="ubuntu" version="8.04-hardy" package="swig"/>
 <sysdepend os="ubuntu" version="7.04-fiesty" package="swig"/>

Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/pre.launch	2009-02-05 23:58:09 UTC (rev 10622)
@@ -1,5 +1,5 @@
 <launch>
-  <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
+  <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/prototype1.xacro.xml'" />
 
   <machine name="xenomai_root" user="root" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>   
 
@@ -9,7 +9,7 @@
   <node machine="xenomai_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i rteth0 -x /robotdesc/pr2"/>
 
 <!-- PR2 Calibration -->
-  <include file="$(find pr2_full)/calibrate.launch" />
+  <include file="$(find pr2_prototype1)/calibrate.launch" />
 
 <!-- Joystick -->
   <param name="joy/deadzone" value="5000"/>

Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml	2009-02-05 23:58:09 UTC (rev 10622)
@@ -9,9 +9,11 @@
   <forearm_calibrator side="r" />
   <gripper_calibrator side="r" />
 
+  <!--
   <upper_arm_calibrator side="l" />
   <forearm_calibrator side="l" />
   <gripper_calibrator side="l" />
+  -->
 
   <base_calibrator />
 

Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml	2009-02-05 23:58:09 UTC (rev 10622)
@@ -161,7 +161,7 @@
       <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
              k_position="100" k_velocity="2"
              safety_length_min="0.15" safety_length_max="0.15" />
-      <calibration reference_position="${reflect*M_PI/2 + upper_arm_roll_cal}" values="1.5 -1 " />
+      <calibration reference_position="${-M_PI/2 + upper_arm_roll_cal}" values="1.5 -1 " />
 
       <joint_properties damping="1.0" />
     </joint>
@@ -697,7 +697,7 @@
       <pid p="5.0" i="0" d="0" iClamp="0" />
     </controller>
 
-    <controller type="WristCalibrationControllerNode" name="cal_${side}_wrist">
+    <controller type="WristCalibrationControllerNode" name="cal_wrist">
       <calibrate transmission="${side}_wrist_trans"
                  actuator_l="${side}_wrist_l_motor" actuator_r="${side}_wrist_r_motor"
                  flex_joint="${side}_wrist_flex_joint" roll_joint="${side}_wrist_roll_joint"

Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/simulators/rosstage/rosstage.cc	2009-02-05 23:58:09 UTC (rev 10622)
@@ -57,7 +57,7 @@
 @section topic ROS topics
 
 Subscribes to (name/type):
-- @b "cmd_vel"/BaseVel : velocity commands to differentially drive the 
+- @b "cmd_vel"/PoseDot : velocity commands to differentially drive the 
 position model.
 
 Publishes to (name / type):
@@ -87,7 +87,7 @@
 #include <std_msgs/LaserScan.h>
 #include <std_msgs/RobotBase2DOdom.h>
 #include <std_msgs/PoseWithRatesStamped.h>
-#include <std_msgs/BaseVel.h>
+#include <std_msgs/PoseDot.h>
 #include <roslib/Time.h>
 
 #include "tf/transform_broadcaster.h"
@@ -99,7 +99,7 @@
 {
   private:
     // Messages that we'll send or receive
-    std_msgs::BaseVel velMsg;
+    std_msgs::PoseDot velMsg;
     std_msgs::LaserScan laserMsg;
     std_msgs::RobotBase2DOdom odomMsg;
     std_msgs::PoseWithRatesStamped groundTruthMsg;
@@ -169,7 +169,7 @@
 {
   boost::mutex::scoped_lock lock(msg_lock);
 
-  this->positionmodel->SetSpeed(this->velMsg.vx, this->velMsg.vy, this->velMsg.vw);
+  this->positionmodel->SetSpeed(this->velMsg.vel.vx, this->velMsg.vel.vy, this->velMsg.ang_vel.vz);
   this->base_last_cmd = this->sim_time;
 }
 

Modified: pkg/trunk/util/filters/CMakeLists.txt
===================================================================
--- pkg/trunk/util/filters/CMakeLists.txt	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/util/filters/CMakeLists.txt	2009-02-05 23:58:09 UTC (rev 10622)
@@ -2,6 +2,8 @@
 include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 rospack(filters)
 
+rospack_add_boost_directories()
+
 rospack_add_gtest(median_test test/test_median.cpp)
 
 # broken, needs to go to the new form factor

Modified: pkg/trunk/util/filters/manifest.xml
===================================================================
--- pkg/trunk/util/filters/manifest.xml	2009-02-05 23:50:06 UTC (rev 10621)
+++ pkg/trunk/util/filters/manifest.xml	2009-02-05...
 
[truncated message content]