Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[21196] pkg/trunk
|
From: <tf...@us...> - 2009-08-09 00:07:01
|
Revision: 21196
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21196&view=rev
Author: tfoote
Date: 2009-08-09 00:06:54 +0000 (Sun, 09 Aug 2009)
Log Message:
-----------
Switching IMU to sensor_msgs/Imu related to #2277
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/stacks/common_msgs/sensor_msgs/msg/Imu.msg
pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
pkg/trunk/stacks/estimation/robot_pose_ekf/manifest.xml
pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp
pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
pkg/trunk/stacks/imu_drivers/imu_node/manifest.xml
pkg/trunk/util/logsetta/imu/imu_extract.cpp
Modified: pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/calibration/wheel_odometry_calibration/manifest.xml 2009-08-09 00:06:54 UTC (rev 21196)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="tf" />
<depend package="nav_msgs" />
+ <depend package="sensor_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-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.cpp 2009-08-09 00:06:54 UTC (rev 21196)
@@ -90,9 +90,9 @@
void odom_calib::imu_callback()
{
_imu_mutex.lock();
- double tmp, yaw; Transform tf;
- poseMsgToTF(_imu.pose_with_rates.pose, tf);
- tf.getBasis().getEulerZYX(yaw, tmp, tmp);
+ double tmp, yaw; btQuaternion tf;
+ quaternionMsgToTF(_imu.orientation, tf);
+ btMatrix3x3(tf).getEulerZYX(yaw, tmp, tmp);
if (!_imu_active){
_imu_begin = yaw;
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-08-09 00:06:54 UTC (rev 21196)
@@ -44,7 +44,7 @@
// messages
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Twist.h"
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "sensor_msgs/Imu.h"
#include "mechanism_msgs/MechanismState.h"
namespace calibration
@@ -80,7 +80,7 @@
// messages to receive
nav_msgs::Odometry _odom;
- geometry_msgs::PoseWithRatesStamped _imu;
+ sensor_msgs::Imu _imu;
mechanism_msgs::MechanismState _mech;
// estimated robot pose message to send
Modified: pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Imu.msg
===================================================================
--- pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Imu.msg 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/common_msgs/sensor_msgs/msg/Imu.msg 2009-08-09 00:06:54 UTC (rev 21196)
@@ -1,7 +1,12 @@
# This is a message to hold IMU data
-Header stamp
+Header header
+
+geometry_msgs/Quaternion orientation
+float64[9] orientation_covarience # Row major about x, y, z axes
+
geometry_msgs/Vector3 angular_velocity
-float64[9] angualar_velocity_covarience # Row major about z, y, x axes
+float64[9] angualar_velocity_covarience # Row major about x, y, z axes
+
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covarience # Row major x, y z
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/include/robot_pose_ekf/odom_estimation_node.h 2009-08-09 00:06:54 UTC (rev 21196)
@@ -47,7 +47,7 @@
// messages
#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Twist.h"
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PoseStamped.h"
#include "deprecated_msgs/VOPose.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
@@ -70,7 +70,7 @@
*/
typedef boost::shared_ptr<nav_msgs::Odometry const> OdomConstPtr;
-typedef boost::shared_ptr<geometry_msgs::PoseWithRatesStamped const> ImuConstPtr;
+typedef boost::shared_ptr<sensor_msgs::Imu const> ImuConstPtr;
typedef boost::shared_ptr<geometry_msgs::Twist const> VelConstPtr;
class OdomEstimationNode
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/manifest.xml
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/manifest.xml 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/manifest.xml 2009-08-09 00:06:54 UTC (rev 21196)
@@ -10,6 +10,7 @@
<depend package="bfl" />
<depend package="std_msgs" />
<depend package="geometry_msgs" />
+<depend package="sensor_msgs" />
<depend package="deprecated_msgs" />
<depend package="nav_msgs" />
<depend package="tf" />
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/src/odom_estimation_node.cpp 2009-08-09 00:06:54 UTC (rev 21196)
@@ -209,7 +209,9 @@
boost::mutex::scoped_lock lock(imu_mutex_);
imu_stamp_ = imu->header.stamp;
imu_time_ = Time::now();
- poseMsgToTF(imu->pose_with_rates.pose, imu_meas_);
+ btQuaternion orientation;
+ quaternionMsgToTF(imu->orientation, orientation);
+ imu_meas_ = btTransform(orientation, btVector3(0,0,0));
my_filter_.addMeasurement(Stamped<Transform>(imu_meas_, imu_stamp_, "imu", "base_footprint"));
// activate imu
Modified: pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp
===================================================================
--- pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/estimation/robot_pose_ekf/test/test_robot_pose_ekf.cpp 2009-08-09 00:06:54 UTC (rev 21196)
@@ -38,7 +38,7 @@
#include <gtest/gtest.h>
#include "ros/node.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "sensor_msgs/Imu.h"
using namespace ros;
@@ -57,7 +57,7 @@
{
public:
geometry_msgs::PoseWithCovarianceStamped ekf_msg_, ekf_begin_;
- geometry_msgs::PoseWithRatesStamped odom_msg_;
+ sensor_msgs::Imu odom_msg_;
double ekf_counter_, odom_counter_;
Time ekf_time_begin_, odom_time_begin_;
Node* node_;
@@ -136,26 +136,17 @@
EXPECT_GT(Duration(ekf_msg_.header.stamp - ekf_time_begin_).toSec(), ekf_duration * 0.85);
// check if ekf time is same as odom time
- EXPECT_LT(Duration(ekf_time_begin_ - odom_time_begin_).toSec(), 1.0);
- EXPECT_GT(Duration(ekf_time_begin_ - odom_time_begin_).toSec(), -1.0);
- EXPECT_LT(Duration(ekf_msg_.header.stamp - odom_msg_.header.stamp).toSec(), 1.0);
- EXPECT_GT(Duration(ekf_msg_.header.stamp - odom_msg_.header.stamp).toSec(), -1.0);
+ EXPECT_NEAR(ekf_time_begin_.toSec(), odom_time_begin_.toSec(), 1.0);
+ EXPECT_NEAR(ekf_msg_.header.stamp.toSec(), odom_msg_.header.stamp.toSec(), 1.0);
// check filter result
- EXPECT_GT(ekf_begin_.pose.pose.position.x, 0.038043 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.pose.position.x, 0.038043 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.pose.position.y, -0.001618 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.pose.position.y, -0.001618 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.pose.position.z, 0.000000 - EPS_trans);
- EXPECT_LT(ekf_begin_.pose.pose.position.z, 0.000000 + EPS_trans);
- EXPECT_GT(ekf_begin_.pose.pose.orientation.x, 0.000000 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.pose.orientation.x, 0.000000 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.pose.orientation.y, 0.000000 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.pose.orientation.y, 0.000000 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.pose.orientation.z, 0.088400 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.pose.orientation.z, 0.088400 + EPS_rot);
- EXPECT_GT(ekf_begin_.pose.pose.orientation.w, 0.996085 - EPS_rot);
- EXPECT_LT(ekf_begin_.pose.pose.orientation.w, 0.996085 + EPS_rot);
+ EXPECT_NEAR(ekf_begin_.pose.pose.position.x, 0.038043, EPS_trans);
+ EXPECT_NEAR(ekf_begin_.pose.pose.position.y, -0.001618, EPS_trans);
+ EXPECT_NEAR(ekf_begin_.pose.pose.position.z, 0.000000, EPS_trans);
+ EXPECT_NEAR(ekf_begin_.pose.pose.orientation.x, 0.000000, EPS_rot);
+ EXPECT_NEAR(ekf_begin_.pose.pose.orientation.y, 0.000000, EPS_rot);
+ EXPECT_NEAR(ekf_begin_.pose.pose.orientation.z, 0.088400, EPS_rot);
+ EXPECT_NEAR(ekf_begin_.pose.pose.orientation.w, 0.996085, EPS_rot);
SUCCEED();
}
Modified: pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc 2009-08-09 00:06:54 UTC (rev 21196)
@@ -41,7 +41,7 @@
@section information Information
-The IMU provides a single message PoseWithRatesStamped messaged at 100Hz
+The IMU provides a single message Imu messaged at 100Hz
which is taken from the 3DMGX2 ACCEL_ANGRATE_ORIENTATION message.
<hr>
@@ -62,7 +62,7 @@
- None
Publishes to (name / type):
-- @b "imu_data"/<a href="../../std_msgs/html/classstd__msgs_1_1PoseWithRatesStamped.html">std_msgs/PoseWithRatesStamped</a> : the imu data
+- @b "imu_data"/<a href="../../sensor_msgs/html/classstd__msgs_1_1Imu.html">sensor_msgs/Imu</a> : the imu data
- @b "/diagnostics"/<a href="../../diagnostic_msgs/html/classrobot__msgs_1_1DiagnosticMessage.html">diagnostic_msgs/DiagnosticMessage</a> : diagnostic status information.
<hr>
@@ -98,7 +98,7 @@
#include "diagnostic_updater/diagnostic_updater.h"
#include "diagnostic_updater/update_functions.h"
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "sensor_msgs/Imu.h"
#include "std_srvs/Empty.h"
#include "imu_node/GetBoolStatus.h"
@@ -113,7 +113,7 @@
{
public:
ms_3dmgx2_driver::IMU imu;
- geometry_msgs::PoseWithRatesStamped reading;
+ sensor_msgs::Imu reading;
string port;
@@ -154,7 +154,7 @@
desired_freq_(100),
freq_diag_(diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05))
{
- imu_data_pub_ = node_handle_.advertise<geometry_msgs::PoseWithRatesStamped>("imu_data", 100);
+ imu_data_pub_ = node_handle_.advertise<sensor_msgs::Imu>("imu_data", 100);
add_offset_serv_ = node_handle_.advertiseService("imu/add_offset", &ImuNode::addOffset, this);
calibrate_serv_ = node_handle_.advertiseService("imu/calibrate", &ImuNode::calibrate, this);
@@ -284,22 +284,22 @@
ROS_WARN("Gathering data took %f ms. Nominal is 10ms.", 1000 * (endtime - starttime));
prevtime = starttime;
- reading.pose_with_rates.acceleration.linear.x = accel[0];
- reading.pose_with_rates.acceleration.linear.y = accel[1];
- reading.pose_with_rates.acceleration.linear.z = accel[2];
+ reading.linear_acceleration.x = accel[0];
+ reading.linear_acceleration.y = accel[1];
+ reading.linear_acceleration.z = accel[2];
- reading.pose_with_rates.velocity.angular.x = angrate[0];
- reading.pose_with_rates.velocity.angular.y = angrate[1];
- reading.pose_with_rates.velocity.angular.z = angrate[2];
+ reading.angular_velocity.x = angrate[0];
+ reading.angular_velocity.y = angrate[1];
+ reading.angular_velocity.z = angrate[2];
- btTransform pose(btMatrix3x3(orientation[0], orientation[1], orientation[2],
- orientation[3], orientation[4], orientation[5],
- orientation[6], orientation[7], orientation[8]),
- btVector3(0,0,0));
-
- tf::poseTFToMsg(pose, reading.pose_with_rates.pose);
+ btQuaternion quat;
+ btMatrix3x3(orientation[0], orientation[1], orientation[2],
+ orientation[3], orientation[4], orientation[5],
+ orientation[6], orientation[7], orientation[8]).getRotation(quat);
+ tf::quaternionTFToMsg(quat, reading.orientation);
+
reading.header.stamp = ros::Time::now().fromNSec(time);
reading.header.frame_id = frameid_;
Modified: pkg/trunk/stacks/imu_drivers/imu_node/manifest.xml
===================================================================
--- pkg/trunk/stacks/imu_drivers/imu_node/manifest.xml 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/stacks/imu_drivers/imu_node/manifest.xml 2009-08-09 00:06:54 UTC (rev 21196)
@@ -6,7 +6,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="roscpp"/>
- <depend package="std_msgs"/>
+ <depend package="sensor_msgs"/>
<depend package="3dmgx2_driver"/>
<depend package="self_test"/>
<depend package="diagnostic_updater"/>
Modified: pkg/trunk/util/logsetta/imu/imu_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/imu/imu_extract.cpp 2009-08-08 23:50:51 UTC (rev 21195)
+++ pkg/trunk/util/logsetta/imu/imu_extract.cpp 2009-08-09 00:06:54 UTC (rev 21196)
@@ -32,19 +32,19 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "geometry_msgs/PoseWithRatesStamped.h"
+#include "sensor_msgs/Imu.h"
#include <string>
#include "rosrecord/Player.h"
-void imu_callback(std::string name, geometry_msgs::PoseWithRatesStamped* imu, ros::Time t, ros::Time t_no_use, void* f)
+void imu_callback(std::string name, sensor_msgs::Imu* imu, ros::Time t, ros::Time t_no_use, void* f)
{
FILE* file = (FILE*)f;
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n",
t.toSec(),
imu->header.stamp.toSec(),
- imu->pose_with_rates.acceleration.linear.x, imu->pose_with_rates.acceleration.linear.y, imu->pose_with_rates.acceleration.linear.z,
- imu->pose_with_rates.velocity.angular.x, imu->pose_with_rates.velocity.angular.y, imu->pose_with_rates.velocity.angular.z);
+ imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z,
+ imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z);
}
int main(int argc, char **argv)
@@ -63,7 +63,7 @@
FILE* file = fopen("imu.txt", "w");
- player.addHandler<geometry_msgs::PoseWithRatesStamped>(std::string("*"), &imu_callback, file);
+ player.addHandler<sensor_msgs::Imu>(std::string("*"), &imu_callback, file);
while(player.nextMsg()) {}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
