How to publish odometry from 3 wheeled omnidirectional robot
Hello,
I'm trying to implement the ROS navigation stack in a 3 wheeled omnidirectional robot. I already made the map using RTABMap but now I'm confused in the 'Publishing Odometry Information over ROS' step. The robot ouputs odometry data in geometry_msgs/Pose2D format.
Here are some of the codes compiled in STM32
//Calculate Odometry
void calculate_odometry(void)
{
short int odometry0_speed = odometry0;
odometry0 = 0;
short int odometry1_speed = odometry1;
odometry1 = 0;
float buffer_x[2];
float buffer_y[2];
buffer_x[0] = odometry0_speed * cosf(gyro_radian + 0.785398);
buffer_x[1] = odometry1_speed * cosf(gyro_radian + 2.356190);
buffer_y[0] = odometry0_speed * sinf(gyro_radian + 0.785398);
buffer_y[1] = odometry1_speed * sinf(gyro_radian + 2.356190);
x_buffer_position += (buffer_x[0] + buffer_x[1]) * odometry_to_cm;
y_buffer_position -= (buffer_y[0] + buffer_y[1]) * odometry_to_cm;
x_position = x_buffer_position - x_offset_position;
y_position = y_buffer_position - y_offset_position;
}
//ROS SUBSCRIBER CALLBACK
void cllbck_sub_velocity(const geometry_msgs::Twist &msg)
{
x_velocity = msg.linear.x;
y_velocity = msg.linear.y;
angular_velocity = msg.angular.z;
motor_timer = 0;
}
//RECEIVE data FROM gyro
void DMA1_Stream1_IRQHandler(void)
{
if (DMA_GetITStatus(DMA1_Stream1, DMA_IT_TCIF1))
{
if (gyro_status == RESET)
{
gyro_status = SET;
buzzer(22, 22);
}
memcpy(&gyro_buffer, gyro_receive + 3, 4);
gyro_angle = (gyro_offset - gyro_buffer);
gyro_radian = (gyro_offset - gyro_buffer) * 0.01745329252;
while (gyro_angle > 180)
gyro_angle -= 360;
while (gyro_angle < -180)
gyro_angle += 360;
while (gyro_radian > 3.14159265359)
gyro_radian -= 6.28318530718;
while (gyro_radian < -3.14159265359)
gyro_radian += 6.28318530718;
USART_DMACmd(USART3, USART_DMAReq_Rx, DISABLE);
USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
DMA_ClearITPendingBit(DMA1_Stream1, DMA_IT_TCIF1);
}
}
From these code I should be able to get
x -> x_position dx -> x_velocity
y -> y_position dy -> y_velocity
A -> gyro_angle ω -> angular_velocity
right?
How can I publish in nav_msgs/Odometry? Do I have all the data I need correctly?
UPDATE
I already converted the format to nav_msgs/Odometry but when I send the 2D Nav Goal the robot just spinning in place doing recovery behaviour and the amcl localization always failed
The code can be found here: https://github.com/Vahzahhun/Robot/blob/master/navigation/mobile_robot_odom/src/mobile_odom.cpp
This is the converted odometry msgs: https://github.com/Vahzahhun/Robot/blob/master/navigation/odom.jpg
This is the rqt graph: https://github.com/Vahzahhun/Robot/blob/master/navigation/rqt_graph.jpg
This is the tf frames: https://github.com/Vahzahhun/Robot/blob/master/navigation/frames.jpg
The launch files can be found here: https://github.com/Vahzahhun/Robot/tree/master/navigation
The local_costmap/obstacles and local_costmap/inflated_obstacles topics don't show up in nav_msgs/GridCells https://github.com/Vahzahhun/Robot/blob/master/navigation/local_costmap_obstacles.jpg and then I use OccupancyGrid instead of GridCells https://github.com/Vahzahhun/Robot/blob/master/navigation/obstaclerviz.jpg
After I send the 2D Nav Goal this is the warnings & errors I get: https://github.com/Vahzahhun/Robot/blob/master/navigation/planner_plan_error.jpg
Please help me troubleshoot this problem
//mobile_robot_configuration.launch
[ WARN] [1560753719.492768443]: [Kinect2Bridge::initCalibration] using sensor defaults for color intrinsic parameters.
[ WARN] [1560753719.492823712]: [Kinect2Bridge::initCalibration] using sensor defaults for ir intrinsic parameters.
[ WARN] [1560753719.492860307]: [Kinect2Bridge::initCalibration] using defaults for rotation and translation.
[ WARN] [1560753719.492903831]: [Kinect2Bridge::initCalibration] using defaults for depth shift.
//move_base.launch
[ WARN] [1560754114.709451440]: Trajectory Rollout planner ...
Excuse me, can you explain about the odometry equation for me? I am a student I am working on a project about this. But I still can't find the answer. thank you.