update

CMakeLists.txt

@@ -10,6 +10,12 @@ project(cps_rmp220_support)
 find_package(catkin REQUIRED COMPONENTS
   urdf
   xacro
+  roscpp
+  rospy
+  std_msgs
+  actionlib
+  move_base_msgs
+  tf
 )
 
 ################################################################################
@@ -38,6 +44,9 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+add_executable(odom_publisher nodes/odom_publisher.cpp)
+target_link_libraries(odom_publisher ${catkin_LIBRARIES})
+
 ################################################################################
 # Install
 ################################################################################

launch/rsp.launch

@@ -9,5 +9,5 @@
     <!-- Start the joint state publisher node -->
     <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" output="screen"/>
 
-    <node pkg="tf" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 odom base_link 40" />
+    <!-- <node pkg="tf" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 odom base_link 40" /> -->
 </launch>

nodes/odom_publisher.cpp

@@ -0,0 +1,90 @@
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <signal.h>
+
+volatile sig_atomic_t flag = 0;
+
+void signal_handler(int signum) {
+  flag = 1;
+}
+
+int main(int argc, char** argv){
+  ros::init(argc, argv, "odometry_publisher");
+
+  // Register the signal handler
+  signal(SIGINT, signal_handler);
+
+  ros::NodeHandle n;
+  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
+  tf::TransformBroadcaster odom_broadcaster;
+
+  double x = 0.0;
+  double y = 0.0;
+  double th = 0.0;
+
+  double vx = 0.1;
+  double vy = -0.1;
+  double vth = 0.1;
+
+  ros::Time current_time, last_time;
+  current_time = ros::Time::now();
+  last_time = ros::Time::now();
+
+  ros::Rate r(1.0);
+  while(ros::ok() && !flag){
+    ros::spinOnce();               // check for incoming messages
+    current_time = ros::Time::now();
+
+    // Compute odometry
+    double dt = (current_time - last_time).toSec();
+    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
+    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
+    double delta_th = vth * dt;
+
+    x += delta_x;
+    y += delta_y;
+    th += delta_th;
+
+    // Create quaternion for orientation
+    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
+
+    // Publish transform over tf
+    geometry_msgs::TransformStamped odom_trans;
+    odom_trans.header.stamp = current_time;
+    odom_trans.header.frame_id = "odom";
+    odom_trans.child_frame_id = "base_link";
+    odom_trans.transform.translation.x = x;
+    odom_trans.transform.translation.y = y;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = odom_quat;
+    odom_broadcaster.sendTransform(odom_trans);
+
+    // Publish odometry message
+    nav_msgs::Odometry odom;
+    odom.header.stamp = current_time;
+    odom.header.frame_id = "odom";
+
+    // Set position and velocity
+    odom.pose.pose.position.x = x;
+    odom.pose.pose.position.y = y;
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = odom_quat;
+
+    odom.child_frame_id = "base_link";
+    odom.twist.twist.linear.x = vx;
+    odom.twist.twist.linear.y = vy;
+    odom.twist.twist.angular.z = vth;
+
+    odom_pub.publish(odom);
+
+    last_time = current_time;
+    r.sleep();
+  }
+
+  ROS_INFO("Odometry node shutting down.");
+  ros::shutdown();
+  return 0;
+}