Replace the Rate loop with a timer callback.

diff --git a/config/display.rviz b/config/display.rviz
deleted file mode 100644 (file)
index dfe64f1..0000000
--- a/config/display.rviz
+++ /dev/null
@@ -1,258 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /TF1
-        - /TF1/Frames1
-      Splitter Ratio: 0.5
-    Tree Height: 549
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz_common/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: Temperature
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      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
-        bno055_shuttle_base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        bno055_shuttle_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        bno055_shuttle_measurement_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        bno055_shuttle_origin:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        bno055_shuttle_socket_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Mass Properties:
-        Inertia: false
-        Mass: false
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Alpha: 0.30000001192092896
-      Autocompute Intensity Bounds: false
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: temperature
-      Class: rviz_default_plugins/Temperature
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: true
-      Max Color: 255; 255; 255
-      Max Intensity: 100
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: Temperature
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.10000000149011612
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        Filter size: 10
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /imu/temperature
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Filter (blacklist): ""
-      Filter (whitelist): ""
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        bno055_shuttle_base_link:
-          Value: false
-        bno055_shuttle_link:
-          Value: false
-        bno055_shuttle_measurement_frame:
-          Value: true
-        bno055_shuttle_origin:
-          Value: false
-        bno055_shuttle_socket_link:
-          Value: false
-        enu_link:
-          Value: true
-        imu_link:
-          Value: false
-        map:
-          Value: false
-      Marker Scale: 0.20000000298023224
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: false
-      Tree:
-        map:
-          enu_link:
-            imu_link:
-              bno055_shuttle_origin:
-                bno055_shuttle_base_link:
-                  bno055_shuttle_socket_link:
-                    bno055_shuttle_link:
-                      bno055_shuttle_measurement_frame:
-                        {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 255; 255; 255
-    Fixed Frame: map
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Covariance x: 0.25
-      Covariance y: 0.25
-      Covariance yaw: 0.06853891909122467
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 0.1501678079366684
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.785398006439209
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 622
-  Y: 87

diff --git a/launch/display.launch.xml b/launch/display.launch.xml
index 7ffc0b4..ffaf071 100644 (file)
--- a/launch/display.launch.xml
+++ b/launch/display.launch.xml
@@ -1,15 +1,35 @@
+<!--****************************************************************************
+                         IMU Fusion Library ROS Wrapper
+                           RViz Launch File
+          Copyright (C) 2019..2025 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        General Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+
+*****************************************************************************-->
+
+
 <launch>
        <arg name="config" default="$(find-pkg-share imufusion_ros)/config/ekf.yaml"/>
 
+       <include file="$(find-pkg-share bno055_driver)/launch/display.launch.xml"/>
+
        <include file="$(find-pkg-share imufusion_ros)/launch/ekf.launch.xml">
                <arg name="config" value="$(var config)"/>
        </include>
 
-       <node pkg="tf2_ros" exec="static_transform_publisher"
-               name="enu_publisher"
-               args="0 0 0 0 0 0.9238795 0.3826834 map enu_link" />
-       <node pkg="imu_odometry" exec="imu2tf" name="imu2tf"
-               args="enu_link" output="screen" />
-       <node name="rviz" pkg="rviz2" exec="rviz2"
-               args="-d $(find-pkg-share imufusion_ros)/config/display.rviz" />
+       <node pkg="imu_odometry" exec="imu2tf" name="ekf2tf" args="enu_link">
+               <remap from="imu/data" to="imu/filtered"/>
+       </node>
 </launch>

diff --git a/launch/ekf.launch.xml b/launch/ekf.launch.xml
index c375626..68a934a 100644 (file)
--- a/launch/ekf.launch.xml
+++ b/launch/ekf.launch.xml
@@ -1,3 +1,24 @@
+<!--****************************************************************************
+                         IMU Fusion Library ROS Wrapper
+                           ekf Launch File
+          Copyright (C) 2019..2025 Walter Fetter Lages <w.fetter@ieee.org>
+
+        This program is free software: you can redistribute it and/or modify
+        it under the terms of the GNU General Public License as published by
+        the Free Software Foundation, either version 3 of the License, or
+        (at your option) any later version.
+
+        This program is distributed in the hope that it will be useful, but
+        WITHOUT ANY WARRANTY; without even the implied warranty of
+        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+        General Public License for more details.
+
+        You should have received a copy of the GNU General Public License
+        along with this program.  If not, see
+        <http://www.gnu.org/licenses/>.
+
+*****************************************************************************-->
+
 <launch>
        <arg name="config" default="$(find-pkg-share imufusion_ros)/config/ekf.yaml"/>
 

diff --git a/src/ekf.cpp b/src/ekf.cpp
index c85ddcf..285f3e3 100644 (file)
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -55,6 +55,8 @@ class EkfNode: public rclcpp::Node
        bool imuAvailable_;
        bool magAvailable_;
 
+       rclcpp::TimerBase::SharedPtr timer_;
+
        void imuCB(const sensor_msgs::msg::Imu::SharedPtr imuMsg);
        void magCB(const sensor_msgs::msg::MagneticField::SharedPtr magMsg);
        
@@ -78,7 +80,7 @@ EkfNode::EkfNode(const char *name,const char *frameId):Node(name)
 
        imuSubscriber_=create_subscription<sensor_msgs::msg::Imu>("imu/data_raw",10,std::bind(&EkfNode::imuCB,this,std::placeholders::_1));
        magSubscriber_=create_subscription<sensor_msgs::msg::MagneticField>("imu/mag",10,std::bind(&EkfNode::magCB,this,std::placeholders::_1));
-       imuPublisher_=create_publisher<sensor_msgs::msg::Imu>("imu/data",10);
+       imuPublisher_=create_publisher<sensor_msgs::msg::Imu>("imu/filtered",10);
 
        Eigen::MatrixXd Pw(7,7);
        Pw.setIdentity();
@@ -105,6 +107,8 @@ EkfNode::EkfNode(const char *name,const char *frameId):Node(name)
        const double T=0.02;    // 0.01 is too fast for an UART at 115200 bps
 
        ekf_=new imufusion::Ekf(Pw,Pv,T,100);
+
+       timer_=rclcpp::create_timer(this,this->get_clock(),std::chrono::milliseconds(static_cast<long int>(1000*T)),std::bind(&EkfNode::update,this));
 }
 
 EkfNode::~EkfNode(void)
@@ -135,6 +139,7 @@ void EkfNode::magCB(const sensor_msgs::msg::MagneticField::SharedPtr magMsg)
 void EkfNode::update(void)
 {
        if(imuAvailable_ && magAvailable_) ekf_->update(accel_,mag_,gyro_);
+       publish();
 }
 
 void EkfNode::publish(void)
@@ -168,16 +173,8 @@ int main(int argc,char* argv[])
                return -1;
        }
        
-       EkfNode ekfNode("ekf",argv[1]);
+       rclcpp::spin(std::make_shared<EkfNode>("ekf",argv[1]));
        
-       rclcpp::Rate loop(50);
-        while(rclcpp::ok())
-        {
-               ekfNode.update();
-               ekfNode.publish();
-                
-                rclcpp::spin_some(ekfNode.get_node_base_interface());
-                loop.sleep();
-       }
+       rclcpp::shutdown();
        return 0;
 }