Update tutorial to newest software architecture

CMakeLists.txt

@@ -1,9 +1,27 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(leo_navigation)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  nav_msgs)
 
-catkin_package()
+catkin_package(
+  CATKIN_DEPENDS
+    tf
+    nav_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(odom2tf
+  src/odom2tf.cpp
+)
+
+target_link_libraries(odom2tf
+  ${catkin_LIBRARIES}
+)
 
 install(
   DIRECTORY
@@ -12,3 +30,7 @@ install(
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+install(TARGETS odom2tf
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+

launch/odometry.launch

@@ -1,24 +1,10 @@
 <launch>
-  <arg name="three_d" default="false" />
+  <arg name="odom_topic" default="odometry_merged"/>
+  <arg name="child_frame_id" default="base_footprint"/>
 
-  <node if="$(arg three_d)"
-    name="imu_filter_node"
-    pkg="imu_filter_madgwick"
-    type="imu_filter_node">
-    <rosparam command="load"
-      file="$(find leo_navigation)/config/imu_filter_node.yaml" />
-  </node>
-
-  <node name="ekf_localization_node"
-    pkg="robot_localization"
-    type="ekf_localization_node"
-    clear_params="true">
-    <rosparam unless="$(arg three_d)"
-      command="load"
-      file="$(find leo_navigation)/config/ekf_localization_node/ekf_2d.yaml" />
-    <rosparam if="$(arg three_d)"
-      command="load"
-      file="$(find leo_navigation)/config/ekf_localization_node/ekf_3d.yaml" />
+  <node name="odom2tf" pkg="leo_navigation" type="odom2tf">
+    <param name="odom_topic" value="$(arg odom_topic)"/>
+    <param name="child_frame_id" value="$(arg child_frame_id)"/>
   </node>
 
 </launch>

package.xml

@@ -12,14 +12,16 @@
   <license>MIT</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <exec_depend>robot_localization</exec_depend>
-  <exec_depend>imu_filter_madgwick</exec_depend>
+  <exec_depend>amcl</exec_depend>
+  <exec_depend>gmapping</exec_depend>
   <exec_depend>slam_gmapping</exec_depend>
   <exec_depend>map_server</exec_depend>
-  <exec_depend>amcl</exec_depend>
   <exec_depend>move_base</exec_depend>
+  <exec_depend>base_local_planner</exec_depend>
+  <exec_depend>dwa_local_planner</exec_depend>
   <exec_depend>global_planner</exec_depend>
   <exec_depend>twist_mux</exec_depend>
-  <exec_depend>gmapping</exec_depend>
+  <depend>tf</depend>
+  <depend>nav_msgs</depend>
 
 </package>

src/odom2tf.cpp

@@ -0,0 +1,32 @@
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+
+static std::string topic = "odometry_merged";
+static std::string child_frame = "base_footprint";
+
+void odom_callback(const nav_msgs::OdometryConstPtr& odom){
+  static tf::TransformBroadcaster br;
+  tf::Transform tf;
+  geometry_msgs::Pose odom_pose = odom->pose.pose;
+  tf::poseMsgToTF(odom_pose, tf);
+
+  tf::StampedTransform stamped_tf(tf, odom->header.stamp, odom->header.frame_id, child_frame);
+
+  br.sendTransform(stamped_tf);
+}
+
+int main(int argc, char **argv){
+
+  ros::init(argc, argv, "odom2tf");
+  ros::NodeHandle nh;
+  ros::NodeHandle pnh("~");
+
+  pnh.getParam("odom_topic", topic);
+  pnh.getParam("child_frame_id", child_frame);
+
+  ros::Subscriber odom_sub = nh.subscribe(topic, 10, odom_callback);
+  ros::spin();
+
+  return 0;
+}