Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23067] pkg/trunk/stacks
|
From: <bla...@us...> - 2009-08-27 00:01:18
|
Revision: 23067
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23067&view=rev
Author: blaisegassend
Date: 2009-08-27 00:01:03 +0000 (Thu, 27 Aug 2009)
Log Message:
-----------
Added covariance data to IMU output, and modified robot launch files
accordingly.
Modified Paths:
--------------
pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
pkg/trunk/stacks/pr2/pr2_alpha/prf.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
Modified: pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc
===================================================================
--- pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc 2009-08-27 00:00:51 UTC (rev 23066)
+++ pkg/trunk/stacks/imu_drivers/imu_node/imu_node.cc 2009-08-27 00:01:03 UTC (rev 23067)
@@ -82,6 +82,9 @@
- @b "~frame_id" : @b [string] the frame in which imu readings will be returned (Default: "imu")
- @b "~autostart" : @b [bool] whether the imu starts on its own (this is only useful for bringing up an imu in test mode)
- @b "~autocalibrate" : @b [bool] whether the imu automatically computes its biases at startup (not useful if you intend to calibrate via the calibrate service)
+ - @b "~orientation_stdev" : @b [double] square root of the orientation_covariance diagonal elements in rad (Default: 0.035)
+ - @b "~angular_velocity_stdev" : @b [double] square root of the angular_velocity_covariance diagonal elements in rad/s (Default: 0.012)
+ - @b "~linear_acceleration_stdev" : @b [double] square root of the linear_acceleration_covariance diagonal elements in m/s^2 (Default: 0.098)
**/
@@ -146,6 +149,10 @@
double bias_y_;
double bias_z_;
+ double angular_velocity_stdev_, angular_velocity_covariance_;
+ double linear_acceleration_covariance_, linear_acceleration_stdev_;
+ double orientation_covariance_, orientation_stdev_;
+
double desired_freq_;
diagnostic_updater::FrequencyStatus freq_diag_;
@@ -173,9 +180,30 @@
bias_x_ = bias_y_ = bias_z_ = 0;
node_handle_.param("~frameid", frameid_, string("imu"));
+ reading.header.frame_id = frameid_;
node_handle_.param("~time_offset", offset_, 0.0);
+ node_handle_.param("~linear_acceleration_stdev", linear_acceleration_stdev_, 0.098);
+ node_handle_.param("~orientation_stdev", orientation_stdev_, 0.035);
+ node_handle_.param("~angular_velocity_stdev", angular_velocity_stdev_, 0.012);
+
+ double angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
+ double orientation_covariance = orientation_stdev_ * orientation_stdev_;
+ double linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
+
+ reading.linear_acceleration_covariance[0] = linear_acceleration_covariance;
+ reading.linear_acceleration_covariance[4] = linear_acceleration_covariance;
+ reading.linear_acceleration_covariance[8] = linear_acceleration_covariance;
+
+ reading.angular_velocity_covariance[0] = angular_velocity_covariance;
+ reading.angular_velocity_covariance[4] = angular_velocity_covariance;
+ reading.angular_velocity_covariance[8] = angular_velocity_covariance;
+
+ reading.orientation_covariance[0] = orientation_covariance;
+ reading.orientation_covariance[4] = orientation_covariance;
+ reading.orientation_covariance[8] = orientation_covariance;
+
self_test_.setPretest(&ImuNode::pretest);
self_test_.addTest(&ImuNode::InterruptionTest);
self_test_.addTest(&ImuNode::ConnectTest);
@@ -299,9 +327,7 @@
tf::quaternionTFToMsg(quat, reading.orientation);
-
reading.header.stamp = ros::Time::now().fromNSec(time);
- reading.header.frame_id = frameid_;
starttime = ros::Time::now().toSec();
imu_data_pub_.publish(reading);
@@ -359,7 +385,7 @@
{
status.name = "Interruption Test";
- if (node_handle_.getNode()->numSubscribers("imu_data") == 0 )
+ if (imu_data_pub_.getNumSubscribers() == 0 )
{
status.level = 0;
status.message = "No operation interrupted.";
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-08-27 00:00:51 UTC (rev 23066)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-08-27 00:01:03 UTC (rev 23067)
@@ -59,6 +59,7 @@
<param name="imu/autostart" type="bool" value="true" />
<param name="imu/frameid" type="string" value="imu" />
<param name="imu/autocalibrate" type="bool" value="true" />
+ <param name="imu/angular_velocity_covariance" type="double" value="0.00017" />
<node machine="realtime" pkg="imu_node" type="imu_node" output="screen"/>
<!-- Forearm Cameras -->
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prf.launch 2009-08-27 00:00:51 UTC (rev 23066)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prf.launch 2009-08-27 00:01:03 UTC (rev 23067)
@@ -53,6 +53,7 @@
<param name="imu/port" type="string" value="/dev/ttyUSB0" />
<param name="imu/autostart" type="bool" value="true" />
<param name="imu/frameid" type="string" value="imu" />
+ <param name="imu/angular_velocity_covariance" type="double" value="0.00017" />
<param name="imu/autocalibrate" type="bool" value="true" />
<node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-08-27 00:00:51 UTC (rev 23066)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-08-27 00:01:03 UTC (rev 23067)
@@ -55,6 +55,7 @@
<param name="imu/port" type="string" value="/dev/ttyUSB0" />
<param name="imu/autostart" type="bool" value="true" />
<param name="imu/frameid" type="string" value="imu" />
+ <param name="imu/angular_velocity_covariance" type="double" value="0.00017" />
<param name="imu/autocalibrate" type="bool" value="true" />
<node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
@@ -95,7 +96,7 @@
<!-- Forearm Camera -->
- <node machine="three" name="forearm_cam_r" pkg="forearm_cam" type="forearm_node" respawn="false" output="screen">
+ <node machine="three" name="forearm_cam_r" pkg="forearm_cam" type="forearm_node" respawn="true" output="screen">
<param name="camera_url" type="str" value="name://forearm_r"/>
<param name="video_mode" type="str" value="640x480x30" />
<param name="auto_exposure" type="bool" value="True" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
