robot_copy.cpp

Go to the documentation of this file.

// std lib
#include <sstream>
#include <iostream>
#include <fstream>
 
// robotics cape library
#include <robotcontrol.h>
 
// ros includes
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <imu_filter_madgwick/imu_filter.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/Float32MultiArray.h>
#include <tf/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Imu.h>
 
// generated headers
#include <robot/RobotConfig.h>
 
// definitions, constants, and macros
#include "robot/robot.h"
 
// reference values (SI units)
float v_left_ref = 0.0;
float v_right_ref = 0.0;
float w_left_ref = 0.0;
float w_right_ref = 0.0;
 
// actual values
int left_encoder = 0;
int left_encoder_previous = 0;
int right_encoder = 0;
int right_encoder_previous = 0;
float position_x = 0.0;
float position_y = 0.0;
float angle_th = 0.0;
float v_robot = 0.0;
float w_robot = 0.0;
float yaw = 0.0;
float pitch = 0.0;
float roll = 0.0;
 
// odom
int odom_source;
rc_mpu_data_t data;
ImuFilter imu_filter;
 
// controller
float integral_left = 0.0;
float integral_right = 0.0;
float kp = 0.0;
float ki = 0.0;
 
// line sensor
float line_values[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 
// pubs
ros::Publisher line_raw_pub;
ros::Publisher twist_pub;
 
ros::Publisher imu_pub;
 
/****************************************************************************************************
 * Functions, procedures
 ****************************************************************************************************/
// rc initialisation
int rc_init()
{
    rc_kill_existing_process(2.0);
    rc_enable_signal_handler();
    rc_make_pid_file();
    rc_motor_init();
    rc_encoder_init();
    rc_adc_init();
    rc_gpio_init(S0, GPIOHANDLE_REQUEST_OUTPUT);
    rc_gpio_init(S1, GPIOHANDLE_REQUEST_OUTPUT);
    rc_gpio_init(S2, GPIOHANDLE_REQUEST_OUTPUT);
    rc_gpio_init(LINE_LED, GPIOHANDLE_REQUEST_OUTPUT);
    rc_mpu_config_t conf = rc_mpu_default_config();
    conf.i2c_bus = 2;
    conf.enable_magnetometer = 1;
    conf.gpio_interrupt_pin_chip = 3;
    conf.gpio_interrupt_pin = 21;
    conf.dmp_sample_rate = 100;      // sample rate
    conf.compass_time_constant = 10; // default 25
    conf.dmp_fetch_accel_gyro = 1;
    conf.mag_sample_rate_div = 1;
    rc_mpu_initialize_dmp(&data, conf);
    return 0;
}
 
// read line sensors
void read_sensors()
{
    try
    {
        // line sensors
        rc_gpio_set_value(LINE_LED, 1);
        rc_usleep(1000);
        for (int i = 1; i <= 6; i++)
        {
            rc_gpio_set_value(S0, bitRead(i, 0));
            rc_gpio_set_value(S1, bitRead(i, 1));
            rc_gpio_set_value(S2, bitRead(i, 2));
            line_values[6 - i] = rc_adc_read_volt(1);
        }
        rc_gpio_set_value(LINE_LED, 0);
        rc_usleep(1000);
        for (int i = 1; i <= 6; i++)
        {
            rc_gpio_set_value(S0, bitRead(i, 0));
            rc_gpio_set_value(S1, bitRead(i, 1));
            rc_gpio_set_value(S2, bitRead(i, 2));
            line_values[6 - i] = line_values[6 - i] - rc_adc_read_volt(1) + 1.8;
        }
        std_msgs::Float32MultiArray line_raw_msg;
        for (int i = 0; i < 6; i++)
        {
            line_raw_msg.data.push_back(line_values[i]);
        }
        line_raw_pub.publish(line_raw_msg);
    }
    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM(e.what());
    }
}
 
/****************************************************************************************************
 * Callbacks
 ****************************************************************************************************/
 
// Twist message callback: sets reference speeds
void twistCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
    v_left_ref = msg->linear.x - msg->angular.z * TRACK_WIDTH_HALF_M;
    v_right_ref = msg->linear.x + msg->angular.z * TRACK_WIDTH_HALF_M;
    w_left_ref = v_left_ref / WHEEL_RADIUS_M;
    w_right_ref = v_right_ref / WHEEL_RADIUS_M;
}
 
// dynamic reconfigure params
void dynamic_callback(robot::RobotConfig &config, uint32_t level)
{
    ROS_INFO_STREAM("Parameter reconfigure requested.");
    kp = config.motor_p;
    ki = config.motor_i;
    odom_source = config.odom_source;
}
 
/****************************************************************************************************
 * Main
 ****************************************************************************************************/
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "robot_node");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("cmd_vel", 1000, twistCallback);
    line_raw_pub = n.advertise<std_msgs::Float32MultiArray>("line_sensors_raw", 1000);
    twist_pub = n.advertise<geometry_msgs::TwistWithCovarianceStamped>("twist_beagle", 1000);
    //
    imu_pub = n.advertise<sensor_msgs::Imu>("imu_beagle", 1000);
 
    static tf::TransformBroadcaster imu_broadcaster;
 
    std::string odom_tf;
    std::string base_link_tf;
    std::string imu_link_tf;
 
 
    if (n.getParam("odom_tf", odom_tf) && n.getParam("base_link_tf", base_link_tf)
        && n.getParam("imu_link_tf", imu_link_tf)){}
    else
    {
      ROS_ERROR("Failed to get param.");
    }
 
    tf::TransformListener listener;
 
    // dynamic reconfigure
    dynamic_reconfigure::Server<robot::RobotConfig> server;
    dynamic_reconfigure::Server<robot::RobotConfig>::CallbackType f;
    f = boost::bind(&dynamic_callback, _1, _2);
    server.setCallback(f);
 
    rc_init();
 
    // filter config
    imu_filter.setAlgorithmGain(0.2);
    // main loop
    ros::Rate loop_rate(SAMPLE_RATE_HZ);
    double start_time = ros::Time::now().toSec();
    ROS_INFO_STREAM("Sensor calibration in progress...");
    // while (ros::ok() && ros::Time::now().toSec() - start_time < 5)
    // {
    //     imu_filter.madgwickAHRSupdate(
    //         data.gyro[1] * DEG_TO_RAD, -data.gyro[0] * DEG_TO_RAD, data.gyro[2] * DEG_TO_RAD,
    //         data.accel[1], -data.accel[0], data.accel[2],
    //         data.mag[1], -data.mag[0], data.mag[2],
    //         DT);
    //     double q0, q1, q2, q3;
    //     imu_filter.getOrientation(q0, q1, q2, q3);
    //     yaw = atan2(2.0f * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
    //     pitch = -asin(2.0f * (q1 * q3 - q0 * q2));
    //     roll = atan2(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
    //     angle_th = yaw;
    //     w_robot = data.gyro[2] * DEG_TO_RAD;
    //     ros::spinOnce();
    //     loop_rate.sleep();
    // }
    ROS_INFO_STREAM("Sensor calibration complete...");
    while (ros::ok())
    {
        // speed measurement
        left_encoder = rc_encoder_read(ENCODER_CHANNEL_L) * ENCODER_POLARITY_L;
        right_encoder = rc_encoder_read(ENCODER_CHANNEL_R) * ENCODER_POLARITY_R;
        float s_left = (left_encoder - left_encoder_previous) * PI * WHEEL_DIAMETER_M / GEARBOX / ENCODER_RES;
        float s_right = (right_encoder - right_encoder_previous) * PI * WHEEL_DIAMETER_M / GEARBOX / ENCODER_RES;
        float v_left = s_left / DT;
        float v_right = s_right / DT;
        float w_left = v_left / WHEEL_RADIUS_M;
        float w_right = v_right / WHEEL_RADIUS_M;
 
        // odometry
        float s_total = (s_left + s_right) / 2.0;
 
        tf::StampedTransform transform;
        try{
          listener.lookupTransform(odom_tf, base_link_tf, ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            }
        angle_th = tf::getYaw(transform.getRotation());
 
        v_robot = s_total / DT;
 
        left_encoder_previous = left_encoder;
        right_encoder_previous = right_encoder;
 
 
        w_robot = (s_right - s_left) / TRACK_WIDTH_M / DT;
        // publihs speed
        ros::Time current_time = ros::Time::now();
        geometry_msgs::TwistWithCovarianceStamped measured_twist;
        measured_twist.header.stamp = current_time;
        measured_twist.header.frame_id = odom_tf;
        measured_twist.twist.twist.linear.x = v_robot * cos(angle_th);
        measured_twist.twist.twist.linear.y = v_robot * sin(angle_th);
        measured_twist.twist.twist.angular.z = w_robot;
 
        measured_twist.twist.covariance[0] = 0.1;
        measured_twist.twist.covariance[7] = 0.1;
        measured_twist.twist.covariance[14] = 0.1;
        measured_twist.twist.covariance[21] = 0.1;
        measured_twist.twist.covariance[28] = 0.1;
        measured_twist.twist.covariance[35] = 0.1;
 
        twist_pub.publish(measured_twist);
 
        // publish base->imu_link transform
        geometry_msgs::TransformStamped imu_trans;
        imu_trans.header.stamp = current_time;
        imu_trans.header.frame_id = base_link_tf;
        imu_trans.child_frame_id = imu_link_tf;
        imu_trans.transform.translation.x = 0.04; // [m]
        imu_trans.transform.translation.y = 0.0;
        imu_trans.transform.translation.z = 0.0;
        imu_trans.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, 0.0); // kao izklopljeno
        imu_broadcaster.sendTransform(imu_trans);
 
        // publish imu
        sensor_msgs::Imu imu;
        imu.header.stamp = current_time;
        imu.header.frame_id = imu_link_tf;
        geometry_msgs::Quaternion imu_quat = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, angle_th);
        //geometry_msgs::Quaternion imu_quat = tf::createQuaternionMsgFromRollPitchYaw(data.fused_TaitBryan[1], -data.fused_TaitBryan[0], data.fused_TaitBryan[2]);
        imu.orientation = imu_quat;
        imu.angular_velocity.x = data.gyro[1] * DEG_TO_RAD;  //vrol
        imu.angular_velocity.y = -data.gyro[0] * DEG_TO_RAD; //vpitch
        imu.angular_velocity.z = data.gyro[2] * DEG_TO_RAD;  //vyaw
        imu.linear_acceleration.x = data.accel[1];           //ax
        imu.linear_acceleration.y = -data.accel[0];          //ay
        imu.linear_acceleration.z = data.accel[2];           //az
 
        imu.orientation_covariance[0] = 0.1;
        imu.orientation_covariance[4] = 0.1;
        imu.orientation_covariance[8] = 0.1;
 
        imu.angular_velocity_covariance[0] = 0.1;
        imu.angular_velocity_covariance[4] = 0.1;
        imu.angular_velocity_covariance[8] = 0.1;
 
        imu.linear_acceleration_covariance[0] = 0.1;
        imu.linear_acceleration_covariance[4] = 0.1;
        imu.linear_acceleration_covariance[8] = 0.1;
 
        imu_pub.publish(imu);
 
        // PID control
        float error_left = w_left_ref - w_left;
        integral_left = constrain(integral_left + error_left * DT, -10.0, 10.0);
        float pwm_left = constrain(kp * error_left + ki * integral_left, -1.0, 1.0);
        float error_right = w_right_ref - w_right;
        integral_right = constrain(integral_right + error_right * DT, -10.0, 10.0);
        float pwm_right = constrain(kp * error_right + ki * integral_right, -1.0, 1.0);
        rc_motor_set(MOTOR_CHANNEL_L, pwm_left * MOTOR_POLARITY_L);
        rc_motor_set(MOTOR_CHANNEL_R, pwm_right * MOTOR_POLARITY_R);
 
        ros::spinOnce();
        loop_rate.sleep();
    }
 
    // rc cleanup
    rc_adc_cleanup();
    rc_gpio_cleanup(S0);
    rc_gpio_cleanup(S1);
    rc_gpio_cleanup(S2);
    rc_gpio_cleanup(LINE_LED);
    rc_encoder_cleanup();
    rc_motor_cleanup();
    rc_remove_pid_file();
 
    return 0;
}