Help starting the LINOROBOT process!

[Ari Mendelow]

Hey guys!

I'm currently working on a 2WD autonomous robot, and with less than two weeks left of our project we have not achieved autonomy. I've just discovered this LINOROBOT project - it's so cool! I wish I had discovered it earlier.

My questions are: will it work with my setup, and can I complete this project in a week?

We are currently using an NVIDIA TX2 running ROS Kinetic, an RPLidar A2, a BNO05 IMU (connected to the TX2 carrier board via I2C), and a motor controller connected to an Arduino Mega (which communicates with the TX2 using rosserial). Our motor controller drives both wheels and has inputs for the speed and direction of each wheel, and that's it. Our drive motors do have encoders, though we aren't sure how to wire them. It looks like they do fall within the specs needed for LINOROBOT, however.
Anyway, can I make the project work with this hardware? Or do I need to purchase the specific hardware specified for this project? And is it possible to complete the project within our time constraints?

If you have any questions or if anything's unclear, let me know.

Thanks!

[Juan Miguel Jimeno]

Hi Ari,

Looking at your components, technically it's possible to build your robot using Linorobot's stack. Most of your components are supported except for your IMU and microcontroller. I advise to split the work into different parts and focus on getting the hardware right.

ARDUINO MEGA
I haven't tried an Arduino Mega on this project personally but I believe someone has successfully ported Linorobot on a Mega. Few things to take note:

1. Connect your encoders on interrupt supported pins on Mega.

2. Remember to remap the pins written on the firmware to the pins you're using on your Mega taking note of point no 1. https://github.com/linorobot/linorobot/blob/master/teensy/firmware/lib/config/lino_base_config.h

3. Make sure you connect MOTOR*_PWM pin to a PWM enabled pin on your Mega for L298 driver and MOTOR*_INA and MOTOR*_INB for BTS7960 motor driver.

4. Edit platformio.ini :
From this: https://github.com/linorobot/linorobot/blob/master/teensy/firmware/platformio.ini
To this:
[env:mega]
platform = atmelavr
framework = arduino
board = megaatmega2560
upload_port = /dev/linobase

IMU
1. The only supported IMU is GY-85. You can tweak the firmware accordingly and use a working code for your IMU and pass it to the variables in publishIMU() function: https://github.com/linorobot/linorobot/blob/master/teensy/firmware/src/firmware.ino#L230

2. Alternitively you can use the encoder data to estimate the robot's angular velocity.

Append:

g_imu_z = vel.angular_z;
to: https://github.com/linorobot/linorobot/blob/master/src/lino_base_node.cpp#L25

and comment the following:
https://github.com/linorobot/linorobot/blob/master/src/lino_base_node.cpp#L40
https://github.com/linorobot/linorobot/blob/master/teensy/firmware/src/firmware.ino#L129-L146

change:
g_imu_dt to to g_vel_dt
https://github.com/linorobot/linorobot/blob/master/src/lino_base_node.cpp#L75

After following these porting instructions you can start going through the tutorial. Make sure to check that the robot's odometry is correct before working on the robot's automomy.

Hope these help all the best!!

[Holopaul]

Hello,
I've been trying to replicate this for about a week. I do not have the GY-85 IMU so i need to get the direction based on the odometry.
I tried to edit the "lino_base_node.cpp" located at  ~/linorobot_ws/src/linorobot/src without success .
When i subscribe to the "odom" topic i get only the linear velocity, not the angular.
What am I doing wrong ?
Thanks

[Juan Miguel Jimeno]

Hi,

Are you trying to port a new IMU or use the motor encoder to estimate the angular velocity?

[Holopaul]

For now I'm using the motor encoder for angular velocity. I'm still having trouble implementing mpu6050.

[Juan Miguel Jimeno]

Did the porting instruction to use encoders for estimating the angular velocity work? MPU6050 is on the list for next version. Hopefully I can release it by early next year.

[Holopaul]

If by instruction you refer to the one in this post, no. I modified the .ino so i don't get the IMU attachment error. I modified the lino_base_node.cpp. But i don't receive the angular velocity in the Odom topic. I still get the the linear values when turning. 

I didn't port to arduino, still running on teensy but i don't have access to the gy-85 IMU.

[Juan Miguel Jimeno]

Did you rebuild the workspace after editing lino_base_node.cpp? Try running catkin_make and rerun  bringup.launch.

If you're still getting the same results, check if angular velocity is published in raw_vel:

rotopic echo raw_vel

[Holopaul]

yes, it's published in raw_vel topic

[Holopaul]

How can i publish it to the odom topic? 

[Holopaul]

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <lino_msgs/Velocities.h>
#include <sensor_msgs/Imu.h>
#include <math.h>

double g_vel_x = 0.0;
double g_vel_y = 0.0;

double g_vel_dt = 0.0;
double g_imu_dt = 0.0;
//double g_imu_z = 0.0;
double g_vel_z = 0.0;

ros::Time g_last_loop_time(0.0);
ros::Time g_last_vel_time(0.0);
ros::Time g_last_imu_time(0.0);

void velCallback( const lino_msgs::Velocities& vel) {
    //callback every time the robot's linear velocity is received
    ros::Time current_time = ros::Time::now();

    g_vel_x = vel.linear_x;
    g_vel_y = vel.linear_y;
    g_imu_z = vel.angular_z;
    g_vel_dt = (current_time - g_last_vel_time).toSec();
    g_last_vel_time = current_time;
}

void IMUCallback( const sensor_msgs::Imu& imu){
    //callback every time the robot's angular velocity is received
    ros::Time current_time = ros::Time::now();
    //this block is to filter out imu noise
    if(imu.angular_velocity.z > -0.03 && imu.angular_velocity.z < 0.03)
    {
        g_imu_z = 0.00;
    }
    else
    {
    //    g_imu_z = imu.angular_velocity.z;
    }
    g_imu_dt = (current_time - g_last_imu_time).toSec();
    g_last_imu_time = current_time;
}

int main(int argc, char** argv){
    ros::init(argc, argv, "base_controller");
    ros::NodeHandle n;
    ros::NodeHandle nh_private_("~");
    ros::Subscriber sub = n.subscribe("raw_vel", 50, velCallback);
    ros::Subscriber imu_sub = n.subscribe("imu/data", 50, IMUCallback);
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;

    double rate = 10.0;
    double x_pos = 0.0;
    double y_pos = 0.0;
    double theta = 0.0;

    ros::Rate r(rate);
    while(n.ok()){
        ros::spinOnce();
        ros::Time current_time = ros::Time::now();

        //linear velocity is the linear velocity published from the Teensy board in x axis
        double linear_velocity_x = g_vel_x;

        //linear velocity is the linear velocity published from the Teensy board in y axis
        double linear_velocity_y = g_vel_y;

        //angular velocity is the rotation in Z from imu_filter_madgwick's output
        double angular_velocity = g_imu_z;

        //calculate angular displacement  θ = ω * t
        double delta_theta = angular_velocity * g_vel_dt; //radians
        double delta_x = (linear_velocity_x * cos(theta) - linear_velocity_y * sin(theta)) * g_vel_dt; //m
        double delta_y = (linear_velocity_x * sin(theta) + linear_velocity_y * cos(theta)) * g_vel_dt; //m

        //calculate current position of the robot
        x_pos += delta_x;
        y_pos += delta_y;
        theta += delta_theta;

        //calculate robot's heading in quarternion angle
        //ROS has a function to calculate yaw in quaternion angle
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);

        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
        //robot's position in x,y, and z
        odom_trans.transform.translation.x = x_pos;
        odom_trans.transform.translation.y = y_pos;
        odom_trans.transform.translation.z = 0.0;
        //robot's heading in quaternion
        odom_trans.transform.rotation = odom_quat;
        odom_trans.header.stamp = current_time;
        //publish robot's tf using odom_trans object
        odom_broadcaster.sendTransform(odom_trans);

        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
        //robot's position in x,y, and z
        odom.pose.pose.position.x = x_pos;
        odom.pose.pose.position.y = y_pos;
        odom.pose.pose.position.z = 0.0;
        //robot's heading in quaternion
        odom.pose.pose.orientation = odom_quat;

        odom.child_frame_id = "base_link";
        //linear speed from encoders
        odom.twist.twist.linear.x = linear_velocity_x;
        odom.twist.twist.linear.y = linear_velocity_y;
        odom.twist.twist.linear.z = 0.0;

        odom.twist.twist.angular.x = 0.0;
        odom.twist.twist.angular.y = 0.0;
        //angular speed from IMU
        odom.twist.twist.angular.z = g_vel_z;

        //TODO: include covariance matrix here

        odom_pub.publish(odom);

        g_last_loop_time = current_time;
        r.sleep();
    }
}

[Juan Miguel Jimeno]

Hi Paul,

Can you try replacing your lino_base_node.cpp with tihs: https://gist.github.com/grassjelly/b06aaf5e73019868236eeb425ca60f76

This has been compiled but not tested. Would be great if you can confirm that the code works once you've tried.

Hope this helps. Cheers!

[Holopaul]

i still get the angular only in raw_vel

[Juan Miguel Jimeno]

did you build it? -

catkin_make

[Holopaul]

of course. 

i will try again with a fresh reboot

[Juan Miguel Jimeno]

Hi Paul,

Did it work for you?

[Holopaul]

Hi Juan, 

Unfortunately it didn't work. I still don't know it's not from my settings. I ordered an IMU, when it will arrive I will try again. 

Thanks again for your help

[ptkpratik]

Hello Juan,

First of all a very big thank you for developing an excellent project, perfect documentation and maintaining an interactive group. For an absolute beginner like me this is pure gold. I am trying to get by hands on ROS by reimplementing the project for 2WD mobile robot. I am trying to implement using Arduino MEGA 2560. As instructed by you in the post I proceeded with the steps you mentioned and right now I am just using the motor encoder to estimate the angular velocity. However, I am running into error as i use platformio to upload the code. I have attached the screenshot.

Thank you once again :)

[Juan Miguel Jimeno]

Hi,

Please take a look at this thread.
https://groups.google.com/forum/m/?utm_medium=email&utm_source=footer#!topic/linorobot/6vlUkd0yAOY

Thanks!

[ptkpratik]

Thank you Juan. Commenting out the ENCODER_OPTIMIZE_INTERRUPTS worked and now I am able to upload the code using platformio. However, as it was mentioned by other user in that thread, I am also not able to use the teleop_twist_keyboard. Could you give an insight of where I could have probably gone wrong or missed something.

[Juan Miguel Jimeno]

Have you commented the following lines? https://github.com/linorobot/linorobot/blob/master/teensy/firmware/src/firmware.ino#L129-L146

If so, can you confirm that the robot is receiving the commands - echo ‘cmd_vel’ topic on the robot’s computer while you’re running teleop from your dev com?

[ptkpratik]

Yes I have commented out the lines related to the IMU. While doing the echo on cmd_vel topic I am seeing the values and these values change as I change the teleop_keyboard input.Also as I press the keys for the teleop I can see the RX led blinking.

Oh and other thing Juan I am doing everything on my PC that is to say i do not have a RPi say as such the robot computer. I am opening new terminal from my PC itself.Is this because the communication ? Like I haven't done the EXPORT_ROS_MASTER things that you did at the beginning of the documentation.

Thanks! :)