Raw to odom msgs

rr_arduino/imu.ino

@@ -38,6 +38,5 @@ void ImuReadings()
   magnetic_msg.magnetic_field.x=mag.x();
   magnetic_msg.magnetic_field.y=mag.y();
   magnetic_msg.magnetic_field.z=mag.z();
-  magnetic_pub(&magnetic_msg);
-
+  magnetic_pub.publish(&magnetic_msg);
 }

rr_arduino/robot_racer/robot_racer.cpp

@@ -4,6 +4,7 @@
  */
 
 #include "robot_racer.h"
+#include <math.h>
 
 /*
  * @brief Constructs Car and assigns values to attributes
@@ -23,6 +24,11 @@ Car::Car()
   forward_throttle_multiplier_ = (MAX_RC_VAL - REST_RC_VAL) / (float)(MANUAL_MAX - NEUTRAL);
   left_steering_multiplier_ = (MAX_RC_STEER_VAL - REST_STEER_VAL) / (float)(MAX_STEERING - STEER_NEUTRAL);
   right_steering_multiplier_ = (MIN_RC_STEER_VAL - REST_STEER_VAL) / (float)(MIN_STEERING - STEER_NEUTRAL);
+  x_ = 0.0;
+  y_ = 0.0;
+  theta_ = 0.0;
+  odom_trans_.header.frame_id = "odom";
+  odom_trans_.child_frame_id = "base_link";
 }
 /*
  *@brief assigns pin values
@@ -280,6 +286,55 @@ void Car::WriteToServos()
 void Car::SetPreviousTime(long time){
 	previous_ = time;
 }
+
 long Car::GetPreviousTime(){
 	return previous_;
 }
+
+void Car::GetOdomTrans() {
+  return odom_trans_;
+}
+
+void Car::GetOdomMsg() {
+  return odom_;
+}
+
+void Car::RawToOdom(float vel, float str_angle) {
+  // Length of the car is 0.335 m
+  double L = 0.335;
+  long current_time = millis();
+  long time_diff = current_time - GetPreviousTime();
+  // If the robot is at the origin, calculate the position using steering angle and the velocity
+  if (str_angle != 0.0 && x_ == 0.0 && y_ == 0.0) {
+    // store str_angle in radians
+    theta_ = str_angle * M_PI / 180.0;
+    x_ = vel * cos(theta_) * time_diff;
+    y_ = vel * sin(theta_) * time_diff;
+
+  } else {
+    // Kinematic equations used: https://nabinsharma.wordpress.com/2014/01/02/kinematics-of-a-robot-bicycle-model/
+    // Calculate turning angle beta
+    double d = vel * time_diff;
+    double R = L / tan(str_angle);
+    double beta = d / R;
+    double xc = x_ - R * sin(theta_);
+    double yc = y_ + R * cos(theta_);
+
+    x_ = xc + R * sin(theta_ + beta);
+    y_ = yc - R * cos(theta_ + beta);
+    theta_ = fmod((theta_ + beta),(2 * M_PI));
+  }
+
+  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_);
+
+  odom_trans_.header.stamp = current_time;
+  odom_trams_.transform.translation.x = x_;
+  odom_trams_.transform.translation.y = y_;
+  odom_trams_.transform.translation.z = 0.0;
+  odom_trams_.transform.rotation = odom_quat;
+
+  odom_.pose.pose.position.x = x_;
+  odom_.pose.pose.position.y = y_;
+  odom_.pose.pose.position.z = 0.0;
+  odom_.pose.pose.orientation = odom_quat;
+}

rr_arduino/robot_racer/robot_racer.h

@@ -3,6 +3,9 @@
 
 #include "Servo.h"
 #include "Arduino.h"
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
 
 // #define BRAKE
 #define TEST_OUTPUT 1
@@ -45,7 +48,7 @@ class Car
 {
   private:
     int RC_signal_[RC_CHANNELS];
-    CarState car_state_;    
+    CarState car_state_;
     unsigned int steering_auto_;
     unsigned int throttle_rc_;
     unsigned int steering_rc_;
@@ -59,7 +62,13 @@ class Car
     float left_steering_multiplier_;
     float right_steering_multiplier_;
     long previous_;
-
+    float x_;
+    float y_;
+    float theta_;
+    geometry_msgs::TransformStamped odom_trans_;
+    nav_msgs::Odometry odom_;
+    tf::TransformBroadcaster odom_broadcaster_;
+
     Servo ThrottleServo_, SteerServo_;
 #ifdef BRAKE
     Servo BrakeServo_;//< brakeServo not used?
@@ -78,5 +87,8 @@ class Car
     void WriteToServos();
     long GetPreviousTime();
     void SetPreviousTime(long time);
+    void GetOdomTrans();
+    void GetOdomMsg();
+    void RawToOdom(float vel, float str_angle);
 };
 #endif

rr_arduino/rr_arduino.ino

@@ -1,12 +1,12 @@
 /*
- * @file Robot Racer Controller                                                       
+ * @file Robot Racer Controller
  * @author Tom Meredith
- * @author Noah Abradjian 
- * @author Toni Ogunmade  
- * @author Abhi Srikantharajah 
+ * @author Noah Abradjian
+ * @author Toni Ogunmade
+ * @author Abhi Srikantharajah
  * @author Brian Kibazohi
  * @competition IARRC 2018
- * Last Updated: June 24, 2018                                                   
+ * Last Updated: June 24, 2018
  */
 
 //#define TEST_OUTPUT
@@ -28,13 +28,11 @@
 #include <sensor_msgs/MagneticField.h>
 #include <sensor_msgs/Imu.h>
 
-
 //Serial, velocity and battery monitoring defines respectively
 const float ROS_BAUD_RATE  =  57600;
-const float IMU_BAUD_RATE  =  9600;
 const float RC_BAUD_RATE   =  115200;
 
-//I2C address for encoder counter 
+//I2C address for encoder counter
 const int SLAVE_ADDRESS = 07;
 
 Adafruit_BNO055 bno = Adafruit_BNO055(55);
@@ -47,7 +45,7 @@ const int      AVERAGING_SIZE    = 5;
 /**
  *@brief function Call
  *publishes actual velocity & average battery value
- *@param current_time 
+ *@param current_time
  *@returns void
  */
 void GetBatteryState(long current_time);
@@ -65,7 +63,7 @@ double rr_velocity = 0.0f , goal_velocity = 0.0f, autonomous_throttle = 1500.0f;
 //brief ROS communication setup begins
 
 //node initialization
- 
+
 ros::NodeHandle nh;
 
 // message objects created
@@ -86,7 +84,6 @@ void CmdVelocityCallback(const std_msgs::Float32 &cmd_vel_msg);
 void CmdSteeringCallback(const std_msgs::Float32 &cmd_str_msg);
 
 
-
 // ROS publisher and subscriber commands
 
 ros::Publisher state_pub("/arduino/vehicle_state", &state_msg);
@@ -99,7 +96,6 @@ ros::Publisher magnetic_pub("/arduino/mag_data", &magnetic_msg);
 ros::Subscriber <std_msgs::Float32> velocity_sub ("/PathPlanner/vel_level", CmdVelocityCallback);
 ros::Subscriber <std_msgs::Float32> steering_sub ("/PathPlanner/steer_cmd", CmdSteeringCallback);
 
-
 int steering_angle = 1500;
 int ROS_watchdog = 0;
 
@@ -114,25 +110,12 @@ void setup() {
 #ifdef TEST_OUTPUT
   Serial.begin(ROS_BAUD_RATE);
 #endif
-  Serial.begin(IMU_BAUD_RATE);
   Serial2.begin(RC_BAUD_RATE);
-  //Initialise the BNO055 sensor
-  if(!bno.begin())
-  {
-    //Detecting BNOO55
-    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
-    while(1);
-  }
-
-  delay(1000);
-
-  bno.setExtCrystalUse(true);
 
 /*
  *ROS Node Handler setup
  *@param specified topics
  */
-  nh.getHardware()->setBaud(ROS_BAUD_RATE);
   nh.initNode();
   nh.advertise(state_pub);
   nh.advertise(encoder_pub);
@@ -144,7 +127,6 @@ void setup() {
   nh.subscribe(velocity_sub);
   nh.subscribe(steering_sub);
 
-
   robot_racer.setup();
 /**
  *PID setup
@@ -153,7 +135,18 @@ void setup() {
  *ThrottlePID.SetSampleTime(20); //<PID algorithm evaluates every 50ms
  *ThrottlePID.SetTunings(333, 0,0.0);
  */
-  pinMode(13,OUTPUT);
+   pinMode(13,OUTPUT);
+   digitalWrite (13, LOW);
+   //if the imu isn't set up properly the LED should come on
+  if(!bno.begin())
+  {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    digitalWrite(13, HIGH);
+  }
+
+  delay(1000);
+
+  bno.setExtCrystalUse(true);
 }
 
 /**
@@ -162,17 +155,20 @@ void setup() {
  *@returns void
  */
 long previous_time = 0; //<for update encoder speed loop
-void loop() {
-  robot_racer.RC_read(); //<get RC controller values
+void loop()
+{
+  //robot_racer.RC_read(); //<get RC controller values
   nh.spinOnce();
+
   long current_time = millis();
   long double time_diff = (current_time - previous_time)/1000.0;
   /*if(Encoder.updateSpeed(time_diff,(float &) rr_velocity))
   {
     previous_time = millis();
     actual_velocity_msg.data = rr_velocity;
-    encoder.publish(&actual_velocity_msg);
+    encoder.publish(&actual_velocity_msg);`
   }*/
+
   switch (robot_racer.GetState()) {
 //case for emergency stop
     case ESTOP:
@@ -184,9 +180,8 @@ void loop() {
       robot_racer.RCMode();
       break;
 
-
 //case for autonomous mode
- 
+
     case AUTO:
       // ThrottlePID.Compute();
       robot_racer.SetThrottle((int)autonomous_throttle);
@@ -199,55 +194,7 @@ void loop() {
   state_pub.publish(&state_msg);
 
   GetBatteryState(current_time);
-  // Get a new sensor event 
-  imu::Vector<3> acc   = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
-  imu::Vector<3> gyro  = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
-  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
-  imu::Vector<3> mag   = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
-
-/* Display the floating point data */
-  Serial.print("XA: ");
-  Serial.print(acc.x());
-  Serial.print(" YA: ");
-  Serial.print(acc.y());
-  Serial.print(" ZA: ");
-  Serial.print(acc.z());
-  Serial.println("\n");
-
-
-/* Display the floating point data */
-  Serial.print(" GX: ");
-  Serial.print(gyro.x());
-  Serial.print(" GY: ");
-  Serial.print(gyro.y());
-  Serial.print(" GZ: ");
-  Serial.print(gyro.z());
-  Serial.println("\n");
-
-
- /* Display the floating point data */
-  Serial.print(" X: ");
-  Serial.print(euler.x());
-  Serial.print("\tY: ");
-  Serial.print(euler.y());
-  Serial.print("\tZ: ");
-  Serial.print(euler.z());
-  Serial.print("\n");
-
-
-  Serial.print(" MX: ");
-  Serial.print(mag.x());
-  Serial.print(" MY: ");
-  Serial.print(mag.y());
-  Serial.print(" MZ: ");
-  Serial.print(mag.z());
-  Serial.println("\n");
-
-
-
-
-  //remove comment on delay to read data on serial monitor
-  //delay(1000);
+
   //call function to publish imu_readings
   ImuReadings();
 }
@@ -263,7 +210,7 @@ void GetBatteryState(long current_time){
     int battery_percentage = 100 * battery_value / 1024;
 
 //Set up array for low pass averaging at first time
- 
+
     static int percentages[AVERAGING_SIZE];
     if (prev_time == 0){
       for (int i = 0; i < AVERAGING_SIZE; i++){
@@ -272,7 +219,7 @@ void GetBatteryState(long current_time){
     }
 
 // update array, calculate sum
- 
+
     int sum = 0;
     for (int i = AVERAGING_SIZE - 1; i > 0; --i){
       percentages[i] = percentages[i - 1];