Skip to content
Snippets Groups Projects
Commit 390c8f10 authored by Fernando Herrero's avatar Fernando Herrero
Browse files

seat_car_controller: modified to match last version of odometry node (v3.1...

seat_car_controller: modified to match last version of odometry node (v3.1 amd64 debs): steering_angle type changed from Int16 to UInt8, and imu yaw expected in rads instead of degrees
parent 16f731cf
No related branches found
No related tags found
No related merge requests found
......@@ -15,6 +15,7 @@
#include <ros/node_handle.h>
#include <std_msgs/Int16.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
......
......@@ -254,7 +254,7 @@ namespace seat_car_controller
this->yaw_pub = root_nh.advertise<std_msgs::Float32>(this->yaw_topic,1);
this->twist_pub = root_nh.advertise<geometry_msgs::Twist>(this->twist_topic,1);
this->steer_angle_pub = root_nh.advertise<std_msgs::Int16>(this->steering_fb_topic,1);
this->steer_angle_pub = root_nh.advertise<std_msgs::UInt8>(this->steering_fb_topic,1);
this->last_cmd_drive=0.0;
this->last_cmd_steer=0.0;
......@@ -272,7 +272,7 @@ namespace seat_car_controller
static double yaw=0.0;
static double last_steer_angle=0.0;
geometry_msgs::Twist twist_msg;
std_msgs::Int16 steer_angle_msg;
std_msgs::UInt8 steer_angle_msg;
steer_l_pos=this->joints_[0].getPosition();
steer_r_pos=this->joints_[1].getPosition();
......@@ -439,7 +439,7 @@ namespace seat_car_controller
std_msgs::Float32 yaw_msg;
this->heading=tf::getYaw(msg->orientation);
yaw_msg.data=-this->heading*180.0/3.14159;
yaw_msg.data=-this->heading; //*180.0/3.14159;
this->yaw_pub.publish(yaw_msg);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment