mirror of
https://github.com/bjoernellens1/cps_rmp220_support.git
synced 2025-01-19 08:06:59 +00:00
91 lines
2.3 KiB
C++
91 lines
2.3 KiB
C++
#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;
|
|
}
|