Commit d765d3c0 authored by jxchenlu's avatar jxchenlu
Browse files

publish lcm odometry message

parent 796f4a35
......@@ -33,6 +33,8 @@ void mb_initialize_odometry(mb_odometry_t* mb_odometry, float x, float y, float
*
*******************************************************************************/
void mb_update_odometry(mb_odometry_t* mb_odometry, mb_state_t* mb_state){
// mb_odometry is the input of previous veh's position and orientation
}
......
......@@ -166,7 +166,7 @@ void read_mb_sensors(){
void publish_mb_msgs(){
mbot_imu_t imu_msg;
mbot_encoder_t encoder_msg;
// odometry_t odo_msg;
odometry_t odo_msg;
//Create IMU LCM Message
imu_msg.utime = now;
......@@ -186,11 +186,15 @@ void publish_mb_msgs(){
encoder_msg.rightticks = mb_state.right_encoder_total;
//TODO: Create Odometry LCM message
odo_msg.utime = now;
odo_msg.x = mb_odometry.x;
odo_msg.y = mb_odometry.y;
odo_msg.theta = mb_odometry.theta;
//publish IMU & Encoder Data to LCM
mbot_imu_t_publish(lcm, MBOT_IMU_CHANNEL, &imu_msg);
mbot_encoder_t_publish(lcm, MBOT_ENCODER_CHANNEL, &encoder_msg);
// odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
odometry_t_publish(lcm, ODOMETRY_CHANNEL, &odo_msg);
}
/*******************************************************************************
......@@ -209,6 +213,9 @@ void publish_mb_msgs(){
void mobilebot_controller(){
update_now();
read_mb_sensors();
// calculate odometry after update reading and defining sensors readings during each call-back
// mb_odometry x,y,theta will be updated in mb_odoemtry.c
mb_update_odometry(&mb_odometry, &mb_state) ;
publish_mb_msgs();
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment