Change topics to comply with ROS REP 145.

diff --git a/launch/display.launch b/launch/display.launch
index 2956a4c..867b2c9 100644 (file)
--- a/launch/display.launch
+++ b/launch/display.launch
@@ -1,10 +1,10 @@
 <launch>
        <include file="$(find bno055_driver)/launch/bno055.launch"/>
-       <include file="$(find s7edge_description)/launch/s7edge.launch"/>
        <node pkg="tf2_ros" type="static_transform_publisher" 
                name="imu_origin_publisher"
-               args="0 0 0 0 0 0 1 map imu" />
-       <node pkg="imu_odometry" type="imu2tf" name="imu2tf" args="origin_link" />
-       <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+               args="0 0 0 0 0 0 1 map imu_link" />
+       <node pkg="imu_odometry" type="imu2tf" name="imu2tf" args="base_link"  output="screen" >
+               <rosparam param="translation">[0.0, 0.0, 0.0]</rosparam>
+       </node>
        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find bno055_driver)/rviz/display.rviz" />
 </launch>

diff --git a/rviz/display.rviz b/rviz/display.rviz
index 8738e7f..3b00f73 100644 (file)
--- a/rviz/display.rviz
+++ b/rviz/display.rviz
@@ -3,11 +3,8 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /RobotModel1
-        - /RobotModel1/Links1
-        - /RobotModel1/Links1/base_link1
-      Splitter Ratio: 0.3928571343421936
+      Expanded: ~
+      Splitter Ratio: 0.4301801919937134
     Tree Height: 558
   - Class: rviz/Selection
     Name: Selection
@@ -27,7 +24,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: ""
+    SyncSource: Temperature
 Preferences:
   PromptSaveOnExit: true
 Visualization Manager:
@@ -52,47 +49,38 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
       Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_link:
-          Alpha: 1
-          Show Axes: true
-          Show Trail: false
-          Value: true
-        origin_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-      Name: RobotModel
-      Robot Description: robot_description
-      TF Prefix: ""
-      Update Interval: 0
+      History Length: 1
+      Name: Imu (data)
+      Topic: /imu/data
+      Unreliable: false
       Value: true
-      Visual Enabled: true
-    - Alpha: 1
+    - Alpha: 0.10000000149011612
       Class: rviz_plugin_tutorials/Imu
-      Color: 204; 51; 204
+      Color: 57; 52; 204
       Enabled: true
       History Length: 1
-      Name: Imu
-      Topic: /imu
+      Name: Imu (raw data)
+      Topic: /imu/data_raw
       Unreliable: false
       Value: true
     - Class: rviz/Axes
       Enabled: true
-      Length: 0.10000000149011612
-      Name: Axes
-      Radius: 0.009999999776482582
+      Length: 5
+      Name: Axes (Fixed Frame)
+      Radius: 0.10000000149011612
       Reference Frame: <Fixed Frame>
       Value: true
-    - Alpha: 1
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 5
+      Name: Axes (base_link)
+      Radius: 0.10000000149011612
+      Reference Frame: base_link
+      Value: true
+    - Alpha: 0.30000001192092896
       Autocompute Intensity Bounds: false
       Autocompute Value Bounds:
         Max Value: 10
@@ -104,7 +92,7 @@ Visualization Manager:
       Color: 255; 255; 255
       Color Transformer: Intensity
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: true
       Max Color: 255; 255; 255
       Max Intensity: 100
@@ -115,13 +103,13 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.10000000149011612
+      Size (m): 1
       Style: Flat Squares
-      Topic: /temperature
+      Topic: /imu/temperature
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 249; 249; 249
@@ -147,16 +135,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.749889850616455
+      Distance: 31.31101417541504
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.19180631637573242
-        Y: -0.2178049087524414
-        Z: 0.2896389067173004
+        X: -0.07251755893230438
+        Y: -0.0821044072508812
+        Z: 0.10933426022529602
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -173,7 +161,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001de000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002b60000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000024200fffffffb0000000800540069006d00650100000000000004500000000000000000000002630000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001be000002b6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002b6000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002b6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002b60000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055c0000003efc0100000002fb0000000800540069006d006501000000000000055c0000024200fffffffb0000000800540069006d00650100000000000004500000000000000000000002920000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -183,5 +171,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1372
-  X: 32
+  X: 16
   Y: 0

diff --git a/src/bno055_node.cpp b/src/bno055_node.cpp
index ab71e3e..024d863 100644 (file)
--- a/src/bno055_node.cpp
+++ b/src/bno055_node.cpp
@@ -33,6 +33,8 @@
 #include <Eigen/Dense>
 using namespace Eigen;
 
+#define sqr(x) ((x)*(x))
+
 class Bno055Node
 {
        public:
@@ -44,10 +46,11 @@ class Bno055Node
        private:
                struct bno055_t bno055_;
                ros::NodeHandle node_;
-               ros::Publisher imuPub_;
+               ros::Publisher dataRawPub_;
+               ros::Publisher dataPub_;
                ros::Publisher temperaturePub_;
                ros::Publisher magneticFieldPub_;
-               ros::Publisher accelPub_;
+               ros::Publisher rpyPub_;
                ros::Publisher gravityPub_;
 
                bool uart_;
@@ -55,12 +58,20 @@ class Bno055Node
                Vector3d accel_;
                Vector3d mag_;
                Vector3d gyro_;
-       
+
                Quaterniond quaternion_;
+               Vector3d rpy_;
                Vector3d linearAccel_;
                Vector3d gravity_;
-       
+
+               double accStdDev_;
+               double gyroStdDev_;
+               double magStdDev_;
+               double orientationStdDev_;
+
                double temp_;
+
+               std::string frameId_;
 };
 
 Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false)
@@ -120,29 +131,40 @@ Bno055Node::Bno055Node(ros::NodeHandle node): uart_(false)
                
        if(bno055_set_gyro_unit(BNO055_GYRO_UNIT_RPS) != BNO055_SUCCESS)
                ROS_ERROR_STREAM("Error setting gyroscope unit to rad/s.");
+
+       if(bno055_set_euler_unit(BNO055_EULER_UNIT_RAD) != BNO055_SUCCESS)
+               ROS_ERROR_STREAM("Error setting Euler angles unit to rad.");
        
        if(bno055_set_temp_unit(BNO055_TEMP_UNIT_CELSIUS) != BNO055_SUCCESS)
                ROS_ERROR_STREAM("Error setting temperature unit to Celsius.");
-       
+
        if(bno055_set_data_output_format(0x01) != BNO055_SUCCESS)
                ROS_ERROR_STREAM("Error setting data output format.");
 
        if(bno055_set_operation_mode(mode) != BNO055_SUCCESS)
                ROS_ERROR_STREAM("Error setting operation mode.");
+
+       nh.param("frame_id",frameId_,std::string("imu_link"));
+       nh.param("linear_acceleration_stddev",accStdDev_,0.0);
+       nh.param("angular_velocity_stddev",gyroStdDev_,0.0);
+       nh.param("magnetic_field_stddev",magStdDev_,0.0);
+       nh.param("orientation_stddev",orientationStdDev_,0.0);
        
-       imuPub_=node_.advertise<sensor_msgs::Imu>("imu",100);
+       dataRawPub_=node_.advertise<sensor_msgs::Imu>("data_raw",100);
+       dataPub_=node_.advertise<sensor_msgs::Imu>("data",100);
        temperaturePub_=node_.advertise<sensor_msgs::Temperature>("temperature",100);
-       magneticFieldPub_=node_.advertise<sensor_msgs::MagneticField>("magnetic_field",100);
-       accelPub_=node_.advertise<geometry_msgs::Vector3Stamped>("acceleration",100);
+       magneticFieldPub_=node_.advertise<sensor_msgs::MagneticField>("mag",100);
+       rpyPub_=node_.advertise<geometry_msgs::Vector3Stamped>("rpy",100);
        gravityPub_=node_.advertise<geometry_msgs::Vector3Stamped>("gravity",100);
 }
 
 Bno055Node::~Bno055Node(void)
 {
-       imuPub_.shutdown();
+       dataRawPub_.shutdown();
+       dataPub_.shutdown();
        temperaturePub_.shutdown();
        magneticFieldPub_.shutdown();
-       accelPub_.shutdown();
+       rpyPub_.shutdown();
        gravityPub_.shutdown();
 
        if(uart_) BNO055_tty_close(&bno055_);
@@ -153,62 +175,72 @@ void Bno055Node::publish(void) const
 {
        ros::Time time=ros::Time::now();
        
-       sensor_msgs::Imu imuMsg;
-       imuMsg.header.stamp=time;
-       imuMsg.header.frame_id="imu";
+       sensor_msgs::Imu dataMsg;
+       dataMsg.header.stamp=time;
+       dataMsg.header.frame_id=frameId_;
        
-       tf::quaternionEigenToMsg(quaternion_,imuMsg.orientation);
-       for (int i=0; i < 9; i++) imuMsg.orientation_covariance[i]=0.0;
-       imuMsg.orientation_covariance[0]=0.0;
-       imuMsg.orientation_covariance[3]=0.0;
-       imuMsg.orientation_covariance[6]=0.0;
-
-       tf::vectorEigenToMsg(gyro_,imuMsg.angular_velocity);
-       for (int i=0; i < 9; i++) imuMsg.angular_velocity_covariance[i]=0.0;
-       imuMsg.angular_velocity_covariance[0]=0.0;
-       imuMsg.angular_velocity_covariance[3]=0.0;
-       imuMsg.angular_velocity_covariance[6]=0.0;
-
-       tf::vectorEigenToMsg(linearAccel_,imuMsg.linear_acceleration);
-       for (int i=0; i < 9; i++) imuMsg.linear_acceleration_covariance[i]=0.0;
-       imuMsg.linear_acceleration_covariance[0]=0.0;
-       imuMsg.linear_acceleration_covariance[3]=0.0;
-       imuMsg.linear_acceleration_covariance[6]=0.0;
-
-       imuPub_.publish(imuMsg);
+       for (int i=0; i < 9; i++) dataMsg.orientation_covariance[i]=-1.0;
+
+       tf::vectorEigenToMsg(gyro_,dataMsg.angular_velocity);
+       for (int i=0; i < 9; i++) dataMsg.angular_velocity_covariance[i]=0.0;
+       dataMsg.angular_velocity_covariance[0]=sqr(gyroStdDev_);
+       dataMsg.angular_velocity_covariance[3]=sqr(gyroStdDev_);
+       dataMsg.angular_velocity_covariance[6]=sqr(gyroStdDev_);
+
+       tf::vectorEigenToMsg(accel_,dataMsg.linear_acceleration);
+       for (int i=0; i < 9; i++) dataMsg.linear_acceleration_covariance[i]=0.0;
+       dataMsg.linear_acceleration_covariance[0]=sqr(accStdDev_);
+       dataMsg.linear_acceleration_covariance[3]=sqr(accStdDev_);
+       dataMsg.linear_acceleration_covariance[6]=sqr(accStdDev_);
+
+       dataRawPub_.publish(dataMsg);
        
+       tf::quaternionEigenToMsg(quaternion_,dataMsg.orientation);
+       for (int i=0; i < 9; i++) dataMsg.orientation_covariance[i]=0.0;
+       dataMsg.orientation_covariance[0]=sqr(orientationStdDev_);
+       dataMsg.orientation_covariance[3]=sqr(orientationStdDev_);
+       dataMsg.orientation_covariance[6]=sqr(orientationStdDev_);
+
+       tf::vectorEigenToMsg(linearAccel_,dataMsg.linear_acceleration);
+       for (int i=0; i < 9; i++) dataMsg.linear_acceleration_covariance[i]=0.0;
+       dataMsg.linear_acceleration_covariance[0]=sqr(accStdDev_);
+       dataMsg.linear_acceleration_covariance[3]=sqr(accStdDev_);
+       dataMsg.linear_acceleration_covariance[6]=sqr(accStdDev_);
+
+       dataPub_.publish(dataMsg);
+
        sensor_msgs::Temperature temperatureMsg;
        temperatureMsg.header.stamp=time;
-       temperatureMsg.header.frame_id="imu";
+       temperatureMsg.header.frame_id=frameId_;
        
        temperatureMsg.temperature=temp_;
        temperatureMsg.variance=0.0;
-       
+
        temperaturePub_.publish(temperatureMsg);
        
        sensor_msgs::MagneticField magneticFieldMsg;
        magneticFieldMsg.header.stamp=time;
-       magneticFieldMsg.header.frame_id="imu";
+       magneticFieldMsg.header.frame_id=frameId_;
        
        tf::vectorEigenToMsg(mag_,magneticFieldMsg.magnetic_field);
        for (int i=0; i < 9; i++) magneticFieldMsg.magnetic_field_covariance[i]=0.0;
-       magneticFieldMsg.magnetic_field_covariance[0]=0.0;
-       magneticFieldMsg.magnetic_field_covariance[3]=0.0;
-       magneticFieldMsg.magnetic_field_covariance[6]=0.0;
+       magneticFieldMsg.magnetic_field_covariance[0]=sqr(magStdDev_);
+       magneticFieldMsg.magnetic_field_covariance[3]=sqr(magStdDev_);
+       magneticFieldMsg.magnetic_field_covariance[6]=sqr(magStdDev_);
 
        magneticFieldPub_.publish(magneticFieldMsg);
        
-       geometry_msgs::Vector3Stamped accelMsg;
-       accelMsg.header.stamp=time;
-       accelMsg.header.frame_id="imu";
+       geometry_msgs::Vector3Stamped rpyMsg;
+       rpyMsg.header.stamp=time;
+       rpyMsg.header.frame_id=frameId_;
        
-       tf::vectorEigenToMsg(accel_,accelMsg.vector);
+       tf::vectorEigenToMsg(rpy_,rpyMsg.vector);
        
-       accelPub_.publish(accelMsg);
+       rpyPub_.publish(rpyMsg);
        
        geometry_msgs::Vector3Stamped gravityMsg;
        gravityMsg.header.stamp=time;
-       gravityMsg.header.frame_id="imu";
+       gravityMsg.header.frame_id=frameId_;
        
        tf::vectorEigenToMsg(accel_,gravityMsg.vector);
        
@@ -239,6 +271,13 @@ void Bno055Node::sample(void)
                        gyro.z/BNO055_GYRO_DIV_RPS;
        else ROS_ERROR_STREAM("Error reading gyroscope.");
 
+       struct bno055_euler_t euler;
+       if(bno055_read_euler_hrp(&euler) == BNO055_SUCCESS)
+                rpy_ << euler.r/BNO055_EULER_DIV_RAD,
+                euler.p/BNO055_EULER_DIV_RAD,
+                euler.h/BNO055_EULER_DIV_RAD;
+        else ROS_ERROR_STREAM("Error reading Euler angles.\n");
+
        s8 temp;
        if(bno055_read_temp_data(&temp) == BNO055_SUCCESS)
                temp_=temp/BNO055_TEMP_DIV_CELSIUS;
@@ -272,11 +311,11 @@ void Bno055Node::sample(void)
 int main(int argc,char* argv[])
 {
        ros::init(argc,argv,"bno055_node");
-       ros::NodeHandle node;
+       ros::NodeHandle node("imu");
        
        Bno055Node bno055Node(node);
        
-       ros::Rate loop(1000);
+       ros::Rate loop(100);
        while(ros::ok())
        {
                bno055Node.sample();