mobile_robot/src/robot.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;
float static_acceleration = 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
// 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;
}
 
/****************************************************************************************************
 * 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);
 
  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.");
  }
 
  // 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();
 
  ros::Duration(0.5).sleep();
 
  static_acceleration = data.accel[0];
 
 
    // filter conf
    // 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;
 
        left_encoder_previous = left_encoder;
        right_encoder_previous = right_encoder;
 
        // 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);
 
        // odometry
        float s_total = (s_left + s_right) / 2.0;
 
        v_robot = s_total / DT;
        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 = base_link_tf;
        measured_twist.twist.twist.linear.x = v_robot; // * cos(angle_th);
        measured_twist.twist.twist.linear.y = 0 ; //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 imu
        sensor_msgs::Imu imu;
        imu.header.stamp = current_time;
        imu.header.frame_id = base_link_tf;
        geometry_msgs::Quaternion imu_quat = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, 0.0);
        //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 = 0;   //vrol
        imu.angular_velocity.y = 0; //vpitch
        imu.angular_velocity.z = data.gyro[2] * DEG_TO_RAD;  //vyaw
        imu.linear_acceleration.x = data.accel[0] - static_acceleration; //* cos(angle_th);           //ax
        imu.linear_acceleration.y = 0.0; // data.accel[1]* sin(angle_th);          //ay
        imu.linear_acceleration.z = 0;           //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);
 
        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;
}