将旧代码仓库迁移至该新仓库中进行维护

arduinoCode/Razor_AHRS/Compass.ino

@@ -0,0 +1,23 @@
+/* This file is part of the Razor AHRS Firmware */
+
+void Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+  
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+  
+  // Tilt compensated magnetic field X
+  mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
+  // Magnetic Heading
+  MAG_Heading = atan2(-mag_y, mag_x);
+}

arduinoCode/Razor_AHRS/DCM.ino

@@ -0,0 +1,127 @@
+/* This file is part of the Razor AHRS Firmware */
+
+// DCM algorithm
+
+/**************************************************/
+void Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  
+  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+  
+  Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+  Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+  
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+/**************************************************/
+void Drift_correction(void)
+{
+  float mag_heading_x;
+  float mag_heading_y;
+  float errorCourse;
+  //Compensation the Roll, Pitch and Yaw drift. 
+  static float Scaled_Omega_P[3];
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+  
+  
+  //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
+  //*****YAW***************
+  // We make the gyro YAW drift correction based on compass magnetic heading
+ 
+  mag_heading_x = cos(MAG_Heading);
+  mag_heading_y = sin(MAG_Heading);
+  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
+  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+  
+  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+
+void Matrix_update(void)
+{
+  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
+  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
+  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
+  
+  Accel_Vector[0]=accel[0];
+  Accel_Vector[1]=accel[1];
+  Accel_Vector[2]=accel[2];
+    
+  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+  
+#if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][2]=0;
+#else // Use drift correction
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+  Update_Matrix[2][2]=0;
+#endif
+
+  Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+  for(int x=0; x<3; x++) //Matrix Addition (update)
+  {
+    for(int y=0; y<3; y++)
+    {
+      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+    } 
+  }
+}
+
+void Euler_angles(void)
+{
+  pitch = -asin(DCM_Matrix[2][0]);
+  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+

arduinoCode/Razor_AHRS/Math.ino

@@ -0,0 +1,89 @@
+/* This file is part of the Razor AHRS Firmware */
+
+// Computes the dot product of two vectors
+float Vector_Dot_Product(const float v1[3], const float v2[3])
+{
+  float result = 0;
+  
+  for(int c = 0; c < 3; c++)
+  {
+    result += v1[c] * v2[c];
+  }
+  
+  return result; 
+}
+
+// Computes the cross product of two vectors
+// out has to different from v1 and v2 (no in-place)!
+void Vector_Cross_Product(float out[3], const float v1[3], const float v2[3])
+{
+  out[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
+  out[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
+  out[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
+}
+
+// Multiply the vector by a scalar
+void Vector_Scale(float out[3], const float v[3], float scale)
+{
+  for(int c = 0; c < 3; c++)
+  {
+    out[c] = v[c] * scale; 
+  }
+}
+
+// Adds two vectors
+void Vector_Add(float out[3], const float v1[3], const float v2[3])
+{
+  for(int c = 0; c < 3; c++)
+  {
+    out[c] = v1[c] + v2[c];
+  }
+}
+
+// Multiply two 3x3 matrices: out = a * b
+// out has to different from a and b (no in-place)!
+void Matrix_Multiply(const float a[3][3], const float b[3][3], float out[3][3])
+{
+  for(int x = 0; x < 3; x++)  // rows
+  {
+    for(int y = 0; y < 3; y++)  // columns
+    {
+      out[x][y] = a[x][0] * b[0][y] + a[x][1] * b[1][y] + a[x][2] * b[2][y];
+    }
+  }
+}
+
+// Multiply 3x3 matrix with vector: out = a * b
+// out has to different from b (no in-place)!
+void Matrix_Vector_Multiply(const float a[3][3], const float b[3], float out[3])
+{
+  for(int x = 0; x < 3; x++)
+  {
+    out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];
+  }
+}
+
+// Init rotation matrix using euler angles
+void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
+{
+  float c1 = cos(roll);
+  float s1 = sin(roll);
+  float c2 = cos(pitch);
+  float s2 = sin(pitch);
+  float c3 = cos(yaw);
+  float s3 = sin(yaw);
+
+  // Euler angles, right-handed, intrinsic, XYZ convention
+  // (which means: rotate around body axes Z, Y', X'') 
+  m[0][0] = c2 * c3;
+  m[0][1] = c3 * s1 * s2 - c1 * s3;
+  m[0][2] = s1 * s3 + c1 * c3 * s2;
+
+  m[1][0] = c2 * s3;
+  m[1][1] = c1 * c3 + s1 * s2 * s3;
+  m[1][2] = c1 * s2 * s3 - c3 * s1;
+
+  m[2][0] = -s2;
+  m[2][1] = c2 * s1;
+  m[2][2] = c1 * c2;
+}

arduinoCode/Razor_AHRS/Output.ino

@@ -0,0 +1,148 @@
+/* This file is part of the Razor AHRS Firmware */
+
+// Output angles: yaw, pitch, roll
+void output_angles()
+{
+  if (output_format == OUTPUT__FORMAT_BINARY)
+  {
+    float ypr[3];  
+    ypr[0] = TO_DEG(yaw);
+    ypr[1] = TO_DEG(pitch);
+    ypr[2] = TO_DEG(roll);
+    Serial.write((byte*) ypr, 12);  // No new-line
+  }
+  else if (output_format == OUTPUT__FORMAT_TEXT)
+  {
+    Serial.print("#YPR=");
+    Serial.print(TO_DEG(yaw)); Serial.print(",");
+    Serial.print(TO_DEG(pitch)); Serial.print(",");
+    Serial.print(TO_DEG(roll)); Serial.println();
+  }
+}
+
+void output_calibration(int calibration_sensor)
+{
+  if (calibration_sensor == 0)  // Accelerometer
+  {
+    // Output MIN/MAX values
+    Serial.print("accel x,y,z (min/max) = ");
+    for (int i = 0; i < 3; i++) {
+      if (accel[i] < accel_min[i]) accel_min[i] = accel[i];
+      if (accel[i] > accel_max[i]) accel_max[i] = accel[i];
+      Serial.print(accel_min[i]);
+      Serial.print("/");
+      Serial.print(accel_max[i]);
+      if (i < 2) Serial.print("  ");
+      else Serial.println();
+    }
+  }
+  else if (calibration_sensor == 1)  // Magnetometer
+  {
+    // Output MIN/MAX values
+    Serial.print("magn x,y,z (min/max) = ");
+    for (int i = 0; i < 3; i++) {
+      if (magnetom[i] < magnetom_min[i]) magnetom_min[i] = magnetom[i];
+      if (magnetom[i] > magnetom_max[i]) magnetom_max[i] = magnetom[i];
+      Serial.print(magnetom_min[i]);
+      Serial.print("/");
+      Serial.print(magnetom_max[i]);
+      if (i < 2) Serial.print("  ");
+      else Serial.println();
+    }
+  }
+  else if (calibration_sensor == 2)  // Gyroscope
+  {
+    // Average gyro values
+    for (int i = 0; i < 3; i++)
+      gyro_average[i] += gyro[i];
+    gyro_num_samples++;
+      
+    // Output current and averaged gyroscope values
+    Serial.print("gyro x,y,z (current/average) = ");
+    for (int i = 0; i < 3; i++) {
+      Serial.print(gyro[i]);
+      Serial.print("/");
+      Serial.print(gyro_average[i] / (float) gyro_num_samples);
+      if (i < 2) Serial.print("  ");
+      else Serial.println();
+    }
+  }
+}
+
+void output_sensors_text(char raw_or_calibrated)
+{
+  Serial.print("#A-"); Serial.print(raw_or_calibrated); Serial.print('=');
+  Serial.print(accel[0]); Serial.print(",");
+  Serial.print(accel[1]); Serial.print(",");
+  Serial.print(accel[2]); Serial.println();
+
+  Serial.print("#M-"); Serial.print(raw_or_calibrated); Serial.print('=');
+  Serial.print(magnetom[0]); Serial.print(",");
+  Serial.print(magnetom[1]); Serial.print(",");
+  Serial.print(magnetom[2]); Serial.println();
+
+  Serial.print("#G-"); Serial.print(raw_or_calibrated); Serial.print('=');
+  Serial.print(gyro[0]); Serial.print(",");
+  Serial.print(gyro[1]); Serial.print(",");
+  Serial.print(gyro[2]); Serial.println();
+}
+
+void output_both_angles_and_sensors_text()
+{
+  Serial.print("#YPRAG=");
+  Serial.print(TO_DEG(yaw)); Serial.print(",");
+  Serial.print(TO_DEG(pitch)); Serial.print(",");
+  Serial.print(TO_DEG(roll)); Serial.print(",");
+  
+  Serial.print(Accel_Vector[0]); Serial.print(",");
+  Serial.print(Accel_Vector[1]); Serial.print(",");
+  Serial.print(Accel_Vector[2]); Serial.print(",");
+
+  Serial.print(Gyro_Vector[0]); Serial.print(",");
+  Serial.print(Gyro_Vector[1]); Serial.print(",");
+  Serial.print(Gyro_Vector[2]); Serial.println();
+}
+
+void output_sensors_binary()
+{
+  Serial.write((byte*) accel, 12);
+  Serial.write((byte*) magnetom, 12);
+  Serial.write((byte*) gyro, 12);
+}
+
+void output_sensors()
+{
+  if (output_mode == OUTPUT__MODE_SENSORS_RAW)
+  {
+    if (output_format == OUTPUT__FORMAT_BINARY)
+      output_sensors_binary();
+    else if (output_format == OUTPUT__FORMAT_TEXT)
+      output_sensors_text('R');
+  }
+  else if (output_mode == OUTPUT__MODE_SENSORS_CALIB)
+  {
+    // Apply sensor calibration
+    compensate_sensor_errors();
+    
+    if (output_format == OUTPUT__FORMAT_BINARY)
+      output_sensors_binary();
+    else if (output_format == OUTPUT__FORMAT_TEXT)
+      output_sensors_text('C');
+  }
+  else if (output_mode == OUTPUT__MODE_SENSORS_BOTH)
+  {
+    if (output_format == OUTPUT__FORMAT_BINARY)
+    {
+      output_sensors_binary();
+      compensate_sensor_errors();
+      output_sensors_binary();
+    }
+    else if (output_format == OUTPUT__FORMAT_TEXT)
+    {
+      output_sensors_text('R');
+      compensate_sensor_errors();
+      output_sensors_text('C');
+    }
+  }
+}
+

arduinoCode/Razor_AHRS/Razor_AHRS.ino

@@ -0,0 +1,849 @@
+/***************************************************************************************************************
+* Razor AHRS Firmware v1.4.2.2
+* 9 Degree of Measurement Attitude and Heading Reference System
+* for Sparkfun "9DOF Razor IMU" (SEN-10125 and SEN-10736)
+* and "9DOF Sensor Stick" (SEN-10183, 10321 and SEN-10724)
+*
+* Released under GNU GPL (General Public License) v3.0
+* Copyright (C) 2013 Peter Bartz [http://ptrbrtz.net]
+* Copyright (C) 2011-2012 Quality & Usability Lab, Deutsche Telekom Laboratories, TU Berlin
+*
+* Infos, updates, bug reports, contributions and feedback:
+*     https://github.com/ptrbrtz/razor-9dof-ahrs
+*
+*
+* History:
+*   * Original code (http://code.google.com/p/sf9domahrs/) by Doug Weibel and Jose Julio,
+*     based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose Julio and Doug Weibel. Thank you!
+*
+*   * Updated code (http://groups.google.com/group/sf_9dof_ahrs_update) by David Malik (david.zsolt.malik@gmail.com)
+*     for new Sparkfun 9DOF Razor hardware (SEN-10125).
+*
+*   * Updated and extended by Peter Bartz (peter-bartz@gmx.de):
+*     * v1.3.0
+*       * Cleaned up, streamlined and restructured most of the code to make it more comprehensible.
+*       * Added sensor calibration (improves precision and responsiveness a lot!).
+*       * Added binary yaw/pitch/roll output.
+*       * Added basic serial command interface to set output modes/calibrate sensors/synch stream/etc.
+*       * Added support to synch automatically when using Rovering Networks Bluetooth modules (and compatible).
+*       * Wrote new easier to use test program (using Processing).
+*       * Added support for new version of "9DOF Razor IMU": SEN-10736.
+*       --> The output of this code is not compatible with the older versions!
+*       --> A Processing sketch to test the tracker is available.
+*     * v1.3.1
+*       * Initializing rotation matrix based on start-up sensor readings -> orientation OK right away.
+*       * Adjusted gyro low-pass filter and output rate settings.
+*     * v1.3.2
+*       * Adapted code to work with new Arduino 1.0 (and older versions still).
+*     * v1.3.3
+*       * Improved synching.
+*     * v1.4.0
+*       * Added support for SparkFun "9DOF Sensor Stick" (versions SEN-10183, SEN-10321 and SEN-10724).
+*     * v1.4.1
+*       * Added output modes to read raw and/or calibrated sensor data in text or binary format.
+*       * Added static magnetometer soft iron distortion compensation
+*     * v1.4.2
+*       * (No core firmware changes)
+*     * v1.4.2.1
+*       * New output mode to support ROS Imu use emits YPR + accel + rot. vel.
+*     * v1.4.2.2
+*       * New input mode to set calibration parameters
+*
+* TODOs:
+*   * Allow optional use of EEPROM for storing and reading calibration values.
+*   * Use self-test and temperature-compensation features of the sensors.
+***************************************************************************************************************/
+
+/*
+  "9DOF Razor IMU" hardware versions: SEN-10125 and SEN-10736
+
+  ATMega328@3.3V, 8MHz
+
+  ADXL345  : Accelerometer
+  HMC5843  : Magnetometer on SEN-10125
+  HMC5883L : Magnetometer on SEN-10736
+  ITG-3200 : Gyro
+
+  Arduino IDE : Select board "Arduino Pro or Pro Mini (3.3v, 8Mhz) w/ATmega328"
+*/
+
+/*
+  "9DOF Sensor Stick" hardware versions: SEN-10183, SEN-10321 and SEN-10724
+
+  ADXL345  : Accelerometer
+  HMC5843  : Magnetometer on SEN-10183 and SEN-10321
+  HMC5883L : Magnetometer on SEN-10724
+  ITG-3200 : Gyro
+*/
+
+/*
+  Axis definition (differs from definition printed on the board!):
+    X axis pointing forward (towards the short edge with the connector holes)
+    Y axis pointing to the right
+    and Z axis pointing down.
+    
+  Positive yaw   : clockwise
+  Positive roll  : right wing down
+  Positive pitch : nose up
+  
+  Transformation order: first yaw then pitch then roll.
+*/
+
+/*
+  Serial commands that the firmware understands:
+  
+  "#c<params>" - SET _c_alibration parameters. The available options are:
+    [a|m|g|c|t] _a_ccelerometer, _m_agnetometer, _g_yro, magnetometerellipsoid_c_enter, magnetometerellipsoid_t_ransform
+    [x|y|z] x,y or z 
+    [m|M|X|Y|Z] _m_in or _M_ax (accel or magnetometer), X, Y, or Z of transform (magnetometerellipsoid_t_ransform)
+
+  "#p" - PRINT current calibration values
+
+  "#o<params>" - Set OUTPUT mode and parameters. The available options are:
+  
+      // Streaming output
+      "#o0" - DISABLE continuous streaming output. Also see #f below.
+      "#o1" - ENABLE continuous streaming output.
+      
+      // Angles output
+      "#ob" - Output angles in BINARY format (yaw/pitch/roll as binary float, so one output frame
+              is 3x4 = 12 bytes long).
+      "#ot" - Output angles in TEXT format (Output frames have form like "#YPR=-142.28,-5.38,33.52",
+              followed by carriage return and line feed [\r\n]).
+      "#ox" - Output angles and linear acceleration and rotational
+              velocity. Angles are in degrees, acceleration is
+              in units of 1.0 = 1/256 G (9.8/256 m/s^2). Rotational
+              velocity is in rad/s^2. (Output frames have form like
+              "#YPRAG=-142.28,-5.38,33.52,0.1,0.1,1.0,0.01,0.01,0.01",
+              followed by carriage return and line feed [\r\n]).
+      // Sensor calibration
+      "#oc" - Go to CALIBRATION output mode.
+      "#on" - When in calibration mode, go on to calibrate NEXT sensor.
+      
+      // Sensor data output
+      "#osct" - Output CALIBRATED SENSOR data of all 9 axes in TEXT format.
+                One frame consist of three lines - one for each sensor: acc, mag, gyr.
+      "#osrt" - Output RAW SENSOR data of all 9 axes in TEXT format.
+                One frame consist of three lines - one for each sensor: acc, mag, gyr.
+      "#osbt" - Output BOTH raw and calibrated SENSOR data of all 9 axes in TEXT format.
+                One frame consist of six lines - like #osrt and #osct combined (first RAW, then CALIBRATED).
+                NOTE: This is a lot of number-to-text conversion work for the little 8MHz chip on the Razor boards.
+                In fact it's too much and an output frame rate of 50Hz can not be maintained. #osbb.
+      "#oscb" - Output CALIBRATED SENSOR data of all 9 axes in BINARY format.
+                One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
+      "#osrb" - Output RAW SENSOR data of all 9 axes in BINARY format.
+                One frame consist of three 3x3 float values = 36 bytes. Order is: acc x/y/z, mag x/y/z, gyr x/y/z.
+      "#osbb" - Output BOTH raw and calibrated SENSOR data of all 9 axes in BINARY format.
+                One frame consist of 2x36 = 72 bytes - like #osrb and #oscb combined (first RAW, then CALIBRATED).
+      
+      // Error message output        
+      "#oe0" - Disable ERROR message output.
+      "#oe1" - Enable ERROR message output.
+    
+    
+  "#f" - Request one output frame - useful when continuous output is disabled and updates are
+         required in larger intervals only. Though #f only requests one reply, replies are still
+         bound to the internal 20ms (50Hz) time raster. So worst case delay that #f can add is 19.99ms.
+         
+         
+  "#s<xy>" - Request synch token - useful to find out where the frame boundaries are in a continuous
+         binary stream or to see if tracker is present and answering. The tracker will send
+         "#SYNCH<xy>\r\n" in response (so it's possible to read using a readLine() function).
+         x and y are two mandatory but arbitrary bytes that can be used to find out which request
+         the answer belongs to.
+          
+          
+  ("#C" and "#D" - Reserved for communication with optional Bluetooth module.)
+  
+  Newline characters are not required. So you could send "#ob#o1#s", which
+  would set binary output mode, enable continuous streaming output and request
+  a synch token all at once.
+  
+  The status LED will be on if streaming output is enabled and off otherwise.
+  
+  Byte order of binary output is little-endian: least significant byte comes first.
+*/
+
+
+
+/*****************************************************************/
+/*********** USER SETUP AREA! Set your options here! *************/
+/*****************************************************************/
+
+// HARDWARE OPTIONS
+/*****************************************************************/
+// Select your hardware here by uncommenting one line!
+//#define HW__VERSION_CODE 10125 // SparkFun "9DOF Razor IMU" version "SEN-10125" (HMC5843 magnetometer)
+#define HW__VERSION_CODE 10736 // SparkFun "9DOF Razor IMU" version "SEN-10736" (HMC5883L magnetometer)
+//#define HW__VERSION_CODE 10183 // SparkFun "9DOF Sensor Stick" version "SEN-10183" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10321 // SparkFun "9DOF Sensor Stick" version "SEN-10321" (HMC5843 magnetometer)
+//#define HW__VERSION_CODE 10724 // SparkFun "9DOF Sensor Stick" version "SEN-10724" (HMC5883L magnetometer)
+
+
+// OUTPUT OPTIONS
+/*****************************************************************/
+// Set your serial port baud rate used to send out data here!
+#define OUTPUT__BAUD_RATE 57600
+
+// Sensor data output interval in milliseconds
+// This may not work, if faster than 20ms (=50Hz)
+// Code is tuned for 20ms, so better leave it like that
+#define OUTPUT__DATA_INTERVAL 20  // in milliseconds
+
+// Output mode definitions (do not change)
+#define OUTPUT__MODE_CALIBRATE_SENSORS 0 // Outputs sensor min/max values as text for manual calibration
+#define OUTPUT__MODE_ANGLES 1 // Outputs yaw/pitch/roll in degrees
+#define OUTPUT__MODE_SENSORS_CALIB 2 // Outputs calibrated sensor values for all 9 axes
+#define OUTPUT__MODE_SENSORS_RAW 3 // Outputs raw (uncalibrated) sensor values for all 9 axes
+#define OUTPUT__MODE_SENSORS_BOTH 4 // Outputs calibrated AND raw sensor values for all 9 axes
+#define OUTPUT__MODE_ANGLES_AG_SENSORS 5 // Outputs yaw/pitch/roll in degrees + linear accel + rot. vel
+// Output format definitions (do not change)
+#define OUTPUT__FORMAT_TEXT 0 // Outputs data as text
+#define OUTPUT__FORMAT_BINARY 1 // Outputs data as binary float
+
+// Select your startup output mode and format here!
+int output_mode = OUTPUT__MODE_ANGLES;
+int output_format = OUTPUT__FORMAT_TEXT;
+
+// Select if serial continuous streaming output is enabled per default on startup.
+#define OUTPUT__STARTUP_STREAM_ON false  // true or false
+
+// If set true, an error message will be output if we fail to read sensor data.
+// Message format: "!ERR: reading <sensor>", followed by "\r\n".
+boolean output_errors = false;  // true or false
+
+// Bluetooth
+// You can set this to true, if you have a Rovering Networks Bluetooth Module attached.
+// The connect/disconnect message prefix of the module has to be set to "#".
+// (Refer to manual, it can be set like this: SO,#)
+// When using this, streaming output will only be enabled as long as we're connected. That way
+// receiver and sender are synchronzed easily just by connecting/disconnecting.
+// It is not necessary to set this! It just makes life easier when writing code for
+// the receiving side. The Processing test sketch also works without setting this.
+// NOTE: When using this, OUTPUT__STARTUP_STREAM_ON has no effect!
+#define OUTPUT__HAS_RN_BLUETOOTH false  // true or false
+
+
+// SENSOR CALIBRATION
+/*****************************************************************/
+// How to calibrate? Read the tutorial at http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs
+// Put MIN/MAX and OFFSET readings for your board here!
+// Accelerometer
+// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+float ACCEL_X_MIN = -250;
+float ACCEL_X_MAX = 250;
+float ACCEL_Y_MIN = -250;
+float ACCEL_Y_MAX = 250;
+float ACCEL_Z_MIN = -250;
+float ACCEL_Z_MAX = 250;
+
+// Magnetometer (standard calibration mode)
+// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
+float MAGN_X_MIN = -600;
+float MAGN_X_MAX = 600;
+float MAGN_Y_MIN = -600;
+float MAGN_Y_MAX = 600;
+float MAGN_Z_MIN = -600;
+float MAGN_Z_MAX = 600;
+
+// Magnetometer (extended calibration mode)
+// Set to true to use extended magnetometer calibration (compensates hard & soft iron errors)
+boolean CALIBRATION__MAGN_USE_EXTENDED = false;
+float magn_ellipsoid_center[3] = {0, 0, 0};
+float magn_ellipsoid_transform[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+// Gyroscope
+// "gyro x,y,z (current/average) = .../OFFSET_X  .../OFFSET_Y  .../OFFSET_Z
+float GYRO_AVERAGE_OFFSET_X = 0.0;
+float GYRO_AVERAGE_OFFSET_Y = 0.0;
+float GYRO_AVERAGE_OFFSET_Z = 0.0;
+
+
+// DEBUG OPTIONS
+/*****************************************************************/
+// When set to true, gyro drift correction will not be applied
+#define DEBUG__NO_DRIFT_CORRECTION false
+// Print elapsed time after each I/O loop
+#define DEBUG__PRINT_LOOP_TIME false
+
+
+/*****************************************************************/
+/****************** END OF USER SETUP AREA!  *********************/
+/*****************************************************************/
+
+
+
+
+
+
+
+
+
+
+// Check if hardware version code is defined
+#ifndef HW__VERSION_CODE
+  // Generate compile error
+  #error YOU HAVE TO SELECT THE HARDWARE YOU ARE USING! See "HARDWARE OPTIONS" in "USER SETUP AREA" at top of Razor_AHRS.ino!
+#endif
+
+#include <Wire.h>
+
+#define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
+
+// Sensor calibration scale and offset values
+float ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f);
+float ACCEL_Y_OFFSET = ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f);
+float ACCEL_Z_OFFSET = ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f);
+float ACCEL_X_SCALE = (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET));
+float ACCEL_Y_SCALE = (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET));
+float ACCEL_Z_SCALE = (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET));
+
+float MAGN_X_OFFSET = ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f);
+float MAGN_Y_OFFSET = ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f);
+float MAGN_Z_OFFSET = ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f);
+float MAGN_X_SCALE = (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET));
+float MAGN_Y_SCALE = (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET));
+float MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET));
+
+
+// Gain for gyroscope (ITG-3200)
+#define GYRO_GAIN 0.06957 // Same gain on all axes
+#define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
+
+// DCM parameters
+#define Kp_ROLLPITCH 0.02f
+#define Ki_ROLLPITCH 0.00002f
+#define Kp_YAW 1.2f
+#define Ki_YAW 0.00002f
+
+// Stuff
+#define STATUS_LED_PIN 13  // Pin number of status LED
+#define TO_RAD(x) (x * 0.01745329252)  // *pi/180
+#define TO_DEG(x) (x * 57.2957795131)  // *180/pi
+
+// Sensor variables
+float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
+float accel_min[3];
+float accel_max[3];
+
+float magnetom[3];
+float magnetom_min[3];
+float magnetom_max[3];
+float magnetom_tmp[3];
+
+float gyro[3];
+float gyro_average[3];
+int gyro_num_samples = 0;
+
+// DCM variables
+float MAG_Heading;
+float Accel_Vector[3]= {0, 0, 0}; // Store the acceleration in a vector
+float Gyro_Vector[3]= {0, 0, 0}; // Store the gyros turn rate in a vector
+float Omega_Vector[3]= {0, 0, 0}; // Corrected Gyro_Vector data
+float Omega_P[3]= {0, 0, 0}; // Omega Proportional correction
+float Omega_I[3]= {0, 0, 0}; // Omega Integrator
+float Omega[3]= {0, 0, 0};
+float errorRollPitch[3] = {0, 0, 0};
+float errorYaw[3] = {0, 0, 0};
+float DCM_Matrix[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+float Update_Matrix[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
+float Temporary_Matrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+
+// Euler angles
+float yaw;
+float pitch;
+float roll;
+
+// DCM timing in the main loop
+unsigned long timestamp;
+unsigned long timestamp_old;
+float G_Dt; // Integration time for DCM algorithm
+
+// More output-state variables
+boolean output_stream_on;
+boolean output_single_on;
+int curr_calibration_sensor = 0;
+boolean reset_calibration_session_flag = true;
+int num_accel_errors = 0;
+int num_magn_errors = 0;
+int num_gyro_errors = 0;
+
+void read_sensors() {
+  Read_Gyro(); // Read gyroscope
+  Read_Accel(); // Read accelerometer
+  Read_Magn(); // Read magnetometer
+}
+
+//should be called after every #ca calibration command
+void recalculateAccelCalibration(){
+  ACCEL_X_OFFSET = ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f);
+  ACCEL_Y_OFFSET = ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f);
+  ACCEL_Z_OFFSET = ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f);
+  ACCEL_X_SCALE = (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET));
+  ACCEL_Y_SCALE = (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET));
+  ACCEL_Z_SCALE = (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET));
+}
+
+//should be called after every #cm calibration command
+void recalculateMagnCalibration(){
+  MAGN_X_OFFSET = ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f);
+  MAGN_Y_OFFSET = ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f);
+  MAGN_Z_OFFSET = ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f);
+  MAGN_X_SCALE = (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET));
+  MAGN_Y_SCALE = (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET));
+  MAGN_Z_SCALE = (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET));
+}
+
+// Read every sensor and record a time stamp
+// Init DCM with unfiltered orientation
+// TODO re-init global vars?
+void reset_sensor_fusion() {
+  float temp1[3];
+  float temp2[3];
+  float xAxis[] = {1.0f, 0.0f, 0.0f};
+
+  read_sensors();
+  timestamp = millis();
+  
+  // GET PITCH
+  // Using y-z-plane-component/x-component of gravity vector
+  pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
+	
+  // GET ROLL
+  // Compensate pitch of gravity vector 
+  Vector_Cross_Product(temp1, accel, xAxis);
+  Vector_Cross_Product(temp2, xAxis, temp1);
+  // Normally using x-z-plane-component/y-component of compensated gravity vector
+  // roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
+  // Since we compensated for pitch, x-z-plane-component equals z-component:
+  roll = atan2(temp2[1], temp2[2]);
+  
+  // GET YAW
+  Compass_Heading();
+  yaw = MAG_Heading;
+  
+  // Init rotation matrix
+  init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
+}
+
+// Apply calibration to raw sensor readings
+void compensate_sensor_errors() {
+  // Compensate accelerometer error
+  accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+  accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+  accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+  
+  // Compensate magnetometer error
+  if (CALIBRATION__MAGN_USE_EXTENDED){
+    for (int i = 0; i < 3; i++)
+      magnetom_tmp[i] = magnetom[i] - magn_ellipsoid_center[i];
+    Matrix_Vector_Multiply(magn_ellipsoid_transform, magnetom_tmp, magnetom);
+  }else{
+    magnetom[0] = (magnetom[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
+    magnetom[1] = (magnetom[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
+    magnetom[2] = (magnetom[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
+  }
+  
+  // Compensate gyroscope error
+  gyro[0] -= GYRO_AVERAGE_OFFSET_X;
+  gyro[1] -= GYRO_AVERAGE_OFFSET_Y;
+  gyro[2] -= GYRO_AVERAGE_OFFSET_Z;
+}
+
+// Reset calibration session if reset_calibration_session_flag is set
+void check_reset_calibration_session()
+{
+  // Raw sensor values have to be read already, but no error compensation applied
+
+  // Reset this calibration session?
+  if (!reset_calibration_session_flag) return;
+  
+  // Reset acc and mag calibration variables
+  for (int i = 0; i < 3; i++) {
+    accel_min[i] = accel_max[i] = accel[i];
+    magnetom_min[i] = magnetom_max[i] = magnetom[i];
+  }
+
+  // Reset gyro calibration variables
+  gyro_num_samples = 0;  // Reset gyro calibration averaging
+  gyro_average[0] = gyro_average[1] = gyro_average[2] = 0.0f;
+  
+  reset_calibration_session_flag = false;
+}
+
+void turn_output_stream_on()
+{
+  output_stream_on = true;
+  digitalWrite(STATUS_LED_PIN, HIGH);
+}
+
+void turn_output_stream_off()
+{
+  output_stream_on = false;
+  digitalWrite(STATUS_LED_PIN, LOW);
+}
+
+// Blocks until another byte is available on serial port
+char readChar()
+{
+  while (Serial.available() < 1) { } // Block
+  return Serial.read();
+}
+
+void setup()
+{
+  // Init serial output
+  Serial.begin(OUTPUT__BAUD_RATE);
+  
+  // Init status LED
+  pinMode (STATUS_LED_PIN, OUTPUT);
+  digitalWrite(STATUS_LED_PIN, LOW);
+
+  // Init sensors
+  delay(50);  // Give sensors enough time to start
+  I2C_Init();
+  Accel_Init();
+  Magn_Init();
+  Gyro_Init();
+  
+  // Read sensors, init DCM algorithm
+  delay(20);  // Give sensors enough time to collect data
+  reset_sensor_fusion();
+
+  // Init output
+#if (OUTPUT__HAS_RN_BLUETOOTH == true) || (OUTPUT__STARTUP_STREAM_ON == false)
+  turn_output_stream_off();
+#else
+  turn_output_stream_on();
+#endif
+}
+
+// Main loop
+void loop()
+{
+  // Read incoming control messages
+  if (Serial.available() >= 2)
+  {
+    if (Serial.read() == '#') // Start of new control message
+    {
+      int command = Serial.read(); // Commands
+      if (command == 'f') // request one output _f_rame
+        output_single_on = true;
+      else if (command == 's') // _s_ynch request
+      {
+        // Read ID
+        byte id[2];
+        id[0] = readChar();
+        id[1] = readChar();
+        
+        // Reply with synch message
+        Serial.print("#SYNCH");
+        Serial.write(id, 2);
+        Serial.println();
+      }
+      else if (command == 'o') // Set _o_utput mode
+      {
+        char output_param = readChar();
+        if (output_param == 'n')  // Calibrate _n_ext sensor
+        {
+          curr_calibration_sensor = (curr_calibration_sensor + 1) % 3;
+          reset_calibration_session_flag = true;
+        }
+        else if (output_param == 't') // Output angles as _t_ext
+        {
+          output_mode = OUTPUT__MODE_ANGLES;
+          output_format = OUTPUT__FORMAT_TEXT;
+        }
+        else if (output_param == 'b') // Output angles in _b_inary format
+        {
+          output_mode = OUTPUT__MODE_ANGLES;
+          output_format = OUTPUT__FORMAT_BINARY;
+        }
+        else if (output_param == 'c') // Go to _c_alibration mode
+        {
+          output_mode = OUTPUT__MODE_CALIBRATE_SENSORS;
+          reset_calibration_session_flag = true;
+        }
+        else if (output_param == 'x') // Go to _c_alibration mode for both sensor and angle comment: Tang
+        {
+          output_mode = OUTPUT__MODE_ANGLES_AG_SENSORS;
+          reset_calibration_session_flag = true;
+        }
+        else if (output_param == 's') // Output _s_ensor values
+        {
+          char values_param = readChar();
+          char format_param = readChar();
+          if (values_param == 'r')  // Output _r_aw sensor values
+            output_mode = OUTPUT__MODE_SENSORS_RAW;
+          else if (values_param == 'c')  // Output _c_alibrated sensor values
+            output_mode = OUTPUT__MODE_SENSORS_CALIB;
+          else if (values_param == 'b')  // Output _b_oth sensor values (raw and calibrated)
+            output_mode = OUTPUT__MODE_SENSORS_BOTH;
+
+          if (format_param == 't') // Output values as _t_text
+            output_format = OUTPUT__FORMAT_TEXT;
+          else if (format_param == 'b') // Output values in _b_inary format
+            output_format = OUTPUT__FORMAT_BINARY;
+        }
+        else if (output_param == '0') // Disable continuous streaming output
+        {
+          turn_output_stream_off();
+          reset_calibration_session_flag = true;
+        }
+        else if (output_param == '1') // Enable continuous streaming output
+        {
+          reset_calibration_session_flag = true;
+          turn_output_stream_on();
+        }
+        else if (output_param == 'e') // _e_rror output settings
+        {
+          char error_param = readChar();
+          if (error_param == '0') output_errors = false;
+          else if (error_param == '1') output_errors = true;
+          else if (error_param == 'c') // get error count
+          {
+            Serial.print("#AMG-ERR:");
+            Serial.print(num_accel_errors); Serial.print(",");
+            Serial.print(num_magn_errors); Serial.print(",");
+            Serial.println(num_gyro_errors);
+          }
+        }
+      }
+      else if (command == 'p') // Set _p_rint calibration values
+      {
+         Serial.print("ACCEL_X_MIN:");Serial.println(ACCEL_X_MIN);
+         Serial.print("ACCEL_X_MAX:");Serial.println(ACCEL_X_MAX);
+         Serial.print("ACCEL_Y_MIN:");Serial.println(ACCEL_Y_MIN);
+         Serial.print("ACCEL_Y_MAX:");Serial.println(ACCEL_Y_MAX);
+         Serial.print("ACCEL_Z_MIN:");Serial.println(ACCEL_Z_MIN);
+         Serial.print("ACCEL_Z_MAX:");Serial.println(ACCEL_Z_MAX);
+         Serial.println(""); 
+         Serial.print("MAGN_X_MIN:");Serial.println(MAGN_X_MIN);
+         Serial.print("MAGN_X_MAX:");Serial.println(MAGN_X_MAX);
+         Serial.print("MAGN_Y_MIN:");Serial.println(MAGN_Y_MIN);
+         Serial.print("MAGN_Y_MAX:");Serial.println(MAGN_Y_MAX);
+         Serial.print("MAGN_Z_MIN:");Serial.println(MAGN_Z_MIN);
+         Serial.print("MAGN_Z_MAX:");Serial.println(MAGN_Z_MAX);
+         Serial.println("");
+         Serial.print("MAGN_USE_EXTENDED:");
+         if (CALIBRATION__MAGN_USE_EXTENDED) 
+           Serial.println("true");
+         else
+           Serial.println("false");
+         Serial.print("magn_ellipsoid_center:[");Serial.print(magn_ellipsoid_center[0],4);Serial.print(",");
+         Serial.print(magn_ellipsoid_center[1],4);Serial.print(",");
+         Serial.print(magn_ellipsoid_center[2],4);Serial.println("]");
+         Serial.print("magn_ellipsoid_transform:[");
+         for(int i = 0; i < 3; i++){
+           Serial.print("[");
+           for(int j = 0; j < 3; j++){
+             Serial.print(magn_ellipsoid_transform[i][j],7);
+             if (j < 2) Serial.print(",");
+           }
+           Serial.print("]");
+           if (i < 2) Serial.print(",");
+         }
+         Serial.println("]");
+         Serial.println(""); 
+         Serial.print("GYRO_AVERAGE_OFFSET_X:");Serial.println(GYRO_AVERAGE_OFFSET_X);
+         Serial.print("GYRO_AVERAGE_OFFSET_Y:");Serial.println(GYRO_AVERAGE_OFFSET_Y);
+         Serial.print("GYRO_AVERAGE_OFFSET_Z:");Serial.println(GYRO_AVERAGE_OFFSET_Z);
+      }
+      else if (command == 'c') // Set _i_nput mode
+      {
+        char input_param = readChar();
+        if (input_param == 'a')  // Calibrate _a_ccelerometer
+        {
+          char axis_param = readChar();
+          char type_param = readChar();
+          float value_param = Serial.parseFloat();
+          if (axis_param == 'x')  // x value
+          {
+            if (type_param == 'm')
+              ACCEL_X_MIN = value_param;
+            else if (type_param == 'M')
+              ACCEL_X_MAX = value_param;
+          }
+          else if (axis_param == 'y')  // y value
+          {
+            if (type_param == 'm')
+              ACCEL_Y_MIN = value_param;
+            else if (type_param == 'M')
+              ACCEL_Y_MAX = value_param;
+          }
+          else if (axis_param == 'z')  // z value
+          {
+            if (type_param == 'm')
+              ACCEL_Z_MIN = value_param;
+            else if (type_param == 'M')
+              ACCEL_Z_MAX = value_param;
+          }
+          recalculateAccelCalibration();
+        }
+        else if (input_param == 'm')  // Calibrate _m_agnetometer (basic)
+        {
+          //disable extended magnetometer calibration
+          CALIBRATION__MAGN_USE_EXTENDED = false;
+          char axis_param = readChar();
+          char type_param = readChar();
+          float value_param = Serial.parseFloat();
+          if (axis_param == 'x')  // x value
+          {
+            if (type_param == 'm')
+              MAGN_X_MIN = value_param;
+            else if (type_param == 'M')
+              MAGN_X_MAX = value_param;
+          }
+          else if (axis_param == 'y')  // y value
+          {
+            if (type_param == 'm')
+              MAGN_Y_MIN = value_param;
+            else if (type_param == 'M')
+              MAGN_Y_MAX = value_param;
+          }
+          else if (axis_param == 'z')  // z value
+          {
+            if (type_param == 'm')
+              MAGN_Z_MIN = value_param;
+            else if (type_param == 'M')
+              MAGN_Z_MAX = value_param;
+          }
+          recalculateMagnCalibration();
+        }
+        else if (input_param == 'c')  // Calibrate magnetometerellipsoid_c_enter (extended)
+        {
+          //enable extended magnetometer calibration
+          CALIBRATION__MAGN_USE_EXTENDED = true;
+          char axis_param = readChar();
+          float value_param = Serial.parseFloat();
+          if (axis_param == 'x')  // x value
+              magn_ellipsoid_center[0] = value_param;
+          else if (axis_param == 'y')  // y value
+              magn_ellipsoid_center[1] = value_param;
+          else if (axis_param == 'z')  // z value
+              magn_ellipsoid_center[2] = value_param;
+        }
+        else if (input_param == 't')  // Calibrate magnetometerellipsoid_t_ransform (extended)
+        {
+          //enable extended magnetometer calibration
+          CALIBRATION__MAGN_USE_EXTENDED = true;
+          char axis_param = readChar();
+          char type_param = readChar();
+          float value_param = Serial.parseFloat();
+          if (axis_param == 'x')  // x value
+          {
+            if (type_param == 'X')
+              magn_ellipsoid_transform[0][0] = value_param;
+            else if (type_param == 'Y')
+              magn_ellipsoid_transform[0][1] = value_param;
+            else if (type_param == 'Z')
+              magn_ellipsoid_transform[0][2] = value_param;
+          }
+          else if (axis_param == 'y')  // y value
+          {
+            if (type_param == 'X')
+              magn_ellipsoid_transform[1][0] = value_param;
+            else if (type_param == 'Y')
+              magn_ellipsoid_transform[1][1] = value_param;
+            else if (type_param == 'Z')
+              magn_ellipsoid_transform[1][2] = value_param;
+          }
+          else if (axis_param == 'z')  // z value
+          {
+            if (type_param == 'X')
+              magn_ellipsoid_transform[2][0] = value_param;
+            else if (type_param == 'Y')
+              magn_ellipsoid_transform[2][1] = value_param;
+            else if (type_param == 'Z')
+              magn_ellipsoid_transform[2][2] = value_param;
+          }
+        }
+        else if (input_param == 'g')  // Calibrate _g_yro 
+        {
+          char axis_param = readChar();
+          float value_param = Serial.parseFloat();
+          if (axis_param == 'x')  // x value
+              GYRO_AVERAGE_OFFSET_X = value_param;
+          else if (axis_param == 'y')  // y value
+              GYRO_AVERAGE_OFFSET_Y = value_param;
+          else if (axis_param == 'z')  // z value
+              GYRO_AVERAGE_OFFSET_Z = value_param;
+         }
+      }
+#if OUTPUT__HAS_RN_BLUETOOTH == true
+      // Read messages from bluetooth module
+      // For this to work, the connect/disconnect message prefix of the module has to be set to "#".
+      else if (command == 'C') // Bluetooth "#CONNECT" message (does the same as "#o1")
+        turn_output_stream_on();
+      else if (command == 'D') // Bluetooth "#DISCONNECT" message (does the same as "#o0")
+        turn_output_stream_off();
+#endif // OUTPUT__HAS_RN_BLUETOOTH == true
+    }
+    else
+    { } // Skip character
+  }
+
+  // Time to read the sensors again?
+  if((millis() - timestamp) >= OUTPUT__DATA_INTERVAL)
+  {
+    timestamp_old = timestamp;
+    timestamp = millis();
+    if (timestamp > timestamp_old)
+      G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+    else G_Dt = 0;
+
+    // Update sensor readings
+    read_sensors();
+
+    if (output_mode == OUTPUT__MODE_CALIBRATE_SENSORS)  // We're in calibration mode
+    {
+      check_reset_calibration_session();  // Check if this session needs a reset
+      if (output_stream_on || output_single_on) output_calibration(curr_calibration_sensor);
+    }
+    else if (output_mode == OUTPUT__MODE_ANGLES)  // Output angles
+    {
+      // Apply sensor calibration
+      compensate_sensor_errors();
+    
+      // Run DCM algorithm
+      Compass_Heading(); // Calculate magnetic heading
+      Matrix_update();
+      Normalize();
+      Drift_correction();
+      Euler_angles();
+      
+      if (output_stream_on || output_single_on) output_angles();
+    }
+    else if (output_mode == OUTPUT__MODE_ANGLES_AG_SENSORS)  // Output angles + accel + rot. vel
+    {
+      // Apply sensor calibration
+      compensate_sensor_errors();
+    
+      // Run DCM algorithm
+      Compass_Heading(); // Calculate magnetic heading
+      Matrix_update();
+      Normalize();
+      Drift_correction();
+      Euler_angles();
+      
+      if (output_stream_on || output_single_on) output_both_angles_and_sensors_text();
+    }
+    else  // Output sensor values
+    {      
+      if (output_stream_on || output_single_on) output_sensors();
+    }
+    
+    output_single_on = false;
+    
+#if DEBUG__PRINT_LOOP_TIME == true
+    Serial.print("loop time (ms) = ");
+    Serial.println(millis() - timestamp);
+#endif
+  }
+#if DEBUG__PRINT_LOOP_TIME == true
+  else
+  {
+    Serial.println("waiting...");
+  }
+#endif
+}

arduinoCode/Razor_AHRS/Sensors.ino

@@ -0,0 +1,210 @@
+/* This file is part of the Razor AHRS Firmware */
+
+// I2C code to read the sensors
+
+// Sensor I2C addresses
+#define ACCEL_ADDRESS ((int) 0x53) // 0x53 = 0xA6 / 2
+#define MAGN_ADDRESS  ((int) 0x1E) // 0x1E = 0x3C / 2
+#define GYRO_ADDRESS  ((int) 0x68) // 0x68 = 0xD0 / 2
+
+// Arduino backward compatibility macros
+#if ARDUINO >= 100
+  #define WIRE_SEND(b) Wire.write((byte) b) 
+  #define WIRE_RECEIVE() Wire.read() 
+#else
+  #define WIRE_SEND(b) Wire.send(b)
+  #define WIRE_RECEIVE() Wire.receive() 
+#endif
+
+
+void I2C_Init()
+{
+  Wire.begin();
+}
+
+void Accel_Init()
+{
+  Wire.beginTransmission(ACCEL_ADDRESS);
+  WIRE_SEND(0x2D);  // Power register
+  WIRE_SEND(0x08);  // Measurement mode
+  Wire.endTransmission();
+  delay(5);
+  Wire.beginTransmission(ACCEL_ADDRESS);
+  WIRE_SEND(0x31);  // Data format register
+  WIRE_SEND(0x08);  // Set to full resolution
+  Wire.endTransmission();
+  delay(5);
+  
+  // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
+  Wire.beginTransmission(ACCEL_ADDRESS);
+  WIRE_SEND(0x2C);  // Rate
+  WIRE_SEND(0x09);  // Set to 50Hz, normal operation
+  Wire.endTransmission();
+  delay(5);
+}
+
+// Reads x, y and z accelerometer registers
+void Read_Accel()
+{
+  int i = 0;
+  byte buff[6];
+  
+  Wire.beginTransmission(ACCEL_ADDRESS); 
+  WIRE_SEND(0x32);  // Send address to read from
+  Wire.endTransmission();
+  
+  Wire.beginTransmission(ACCEL_ADDRESS);
+  Wire.requestFrom(ACCEL_ADDRESS, 6);  // Request 6 bytes
+  while(Wire.available())  // ((Wire.available())&&(i<6))
+  { 
+    buff[i] = WIRE_RECEIVE();  // Read one byte
+    i++;
+  }
+  Wire.endTransmission();
+  
+  if (i == 6)  // All bytes received?
+  {
+    // No multiply by -1 for coordinate system transformation here, because of double negation:
+    // We want the gravity vector, which is negated acceleration vector.
+    accel[0] = (((int) buff[3]) << 8) | buff[2];  // X axis (internal sensor y axis)
+    accel[1] = (((int) buff[1]) << 8) | buff[0];  // Y axis (internal sensor x axis)
+    accel[2] = (((int) buff[5]) << 8) | buff[4];  // Z axis (internal sensor z axis)
+  }
+  else
+  {
+    num_accel_errors++;
+    if (output_errors) Serial.println("!ERR: reading accelerometer");
+  }
+}
+
+void Magn_Init()
+{
+  Wire.beginTransmission(MAGN_ADDRESS);
+  WIRE_SEND(0x02); 
+  WIRE_SEND(0x00);  // Set continuous mode (default 10Hz)
+  Wire.endTransmission();
+  delay(5);
+
+  Wire.beginTransmission(MAGN_ADDRESS);
+  WIRE_SEND(0x00);
+  WIRE_SEND(0b00011000);  // Set 50Hz
+  Wire.endTransmission();
+  delay(5);
+}
+
+void Read_Magn()
+{
+  int i = 0;
+  byte buff[6];
+ 
+  Wire.beginTransmission(MAGN_ADDRESS); 
+  WIRE_SEND(0x03);  // Send address to read from
+  Wire.endTransmission();
+  
+  Wire.beginTransmission(MAGN_ADDRESS); 
+  Wire.requestFrom(MAGN_ADDRESS, 6);  // Request 6 bytes
+  while(Wire.available())  // ((Wire.available())&&(i<6))
+  { 
+    buff[i] = WIRE_RECEIVE();  // Read one byte
+    i++;
+  }
+  Wire.endTransmission();
+  
+  if (i == 6)  // All bytes received?
+  {
+// 9DOF Razor IMU SEN-10125 using HMC5843 magnetometer
+#if HW__VERSION_CODE == 10125
+    // MSB byte first, then LSB; X, Y, Z
+    magnetom[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // X axis (internal sensor -y axis)
+    magnetom[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);  // Y axis (internal sensor -x axis)
+    magnetom[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Z axis (internal sensor -z axis)
+// 9DOF Razor IMU SEN-10736 using HMC5883L magnetometer
+#elif HW__VERSION_CODE == 10736
+    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
+    magnetom[0] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // X axis (internal sensor -y axis)
+    magnetom[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);  // Y axis (internal sensor -x axis)
+    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
+// 9DOF Sensor Stick SEN-10183 and SEN-10321 using HMC5843 magnetometer
+#elif (HW__VERSION_CODE == 10183) || (HW__VERSION_CODE == 10321)
+    // MSB byte first, then LSB; X, Y, Z
+    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
+    magnetom[1] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Y axis (internal sensor -y axis)
+    magnetom[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Z axis (internal sensor -z axis)
+// 9DOF Sensor Stick SEN-10724 using HMC5883L magnetometer
+#elif HW__VERSION_CODE == 10724
+    // MSB byte first, then LSB; Y and Z reversed: X, Z, Y
+    magnetom[0] = (((int) buff[0]) << 8) | buff[1];         // X axis (internal sensor x axis)
+    magnetom[1] = -1 * ((((int) buff[4]) << 8) | buff[5]);  // Y axis (internal sensor -y axis)
+    magnetom[2] = -1 * ((((int) buff[2]) << 8) | buff[3]);  // Z axis (internal sensor -z axis)
+#endif
+  }
+  else
+  {
+    num_magn_errors++;
+    if (output_errors) Serial.println("!ERR: reading magnetometer");
+  }
+}
+
+void Gyro_Init()
+{
+  // Power up reset defaults
+  Wire.beginTransmission(GYRO_ADDRESS);
+  WIRE_SEND(0x3E);
+  WIRE_SEND(0x80);
+  Wire.endTransmission();
+  delay(5);
+  
+  // Select full-scale range of the gyro sensors
+  // Set LP filter bandwidth to 42Hz
+  Wire.beginTransmission(GYRO_ADDRESS);
+  WIRE_SEND(0x16);
+  WIRE_SEND(0x1B);  // DLPF_CFG = 3, FS_SEL = 3
+  Wire.endTransmission();
+  delay(5);
+  
+  // Set sample rato to 50Hz
+  Wire.beginTransmission(GYRO_ADDRESS);
+  WIRE_SEND(0x15);
+  WIRE_SEND(0x0A);  //  SMPLRT_DIV = 10 (50Hz)
+  Wire.endTransmission();
+  delay(5);
+
+  // Set clock to PLL with z gyro reference
+  Wire.beginTransmission(GYRO_ADDRESS);
+  WIRE_SEND(0x3E);
+  WIRE_SEND(0x00);
+  Wire.endTransmission();
+  delay(5);
+}
+
+// Reads x, y and z gyroscope registers
+void Read_Gyro()
+{
+  int i = 0;
+  byte buff[6];
+  
+  Wire.beginTransmission(GYRO_ADDRESS); 
+  WIRE_SEND(0x1D);  // Sends address to read from
+  Wire.endTransmission();
+  
+  Wire.beginTransmission(GYRO_ADDRESS);
+  Wire.requestFrom(GYRO_ADDRESS, 6);  // Request 6 bytes
+  while(Wire.available())  // ((Wire.available())&&(i<6))
+  { 
+    buff[i] = WIRE_RECEIVE();  // Read one byte
+    i++;
+  }
+  Wire.endTransmission();
+  
+  if (i == 6)  // All bytes received?
+  {
+    gyro[0] = -1 * ((((int) buff[2]) << 8) | buff[3]);    // X axis (internal sensor -y axis)
+    gyro[1] = -1 * ((((int) buff[0]) << 8) | buff[1]);    // Y axis (internal sensor -x axis)
+    gyro[2] = -1 * ((((int) buff[4]) << 8) | buff[5]);    // Z axis (internal sensor -z axis)
+  }
+  else
+  {
+    num_gyro_errors++;
+    if (output_errors) Serial.println("!ERR: reading gyroscope");
+  }
+}

arduinoCode/mobileBase/commands.h

@@ -0,0 +1,28 @@
+/***********************************************************************************
+Description:Define single-letter commands that will be sent by the PC over the
+   serial link.
+Author: www.corvin.cn
+History: 20180209:init this file,add CODE_VERSION command;
+************************************************************************************/
+
+#ifndef _COMMANDS_H_
+#define _COMMANDS_H_
+
+#define ANALOG_READ    'a'
+#define GET_BAUDRATE   'b'
+#define PIN_MODE       'c'
+#define DIGITAL_READ   'd'
+#define READ_ENCODERS  'e'
+#define MOTOR_SPEEDS   'm'
+#define RESET_ENCODERS 'r'
+#define UPDATE_PID     'u'
+#define DIGITAL_WRITE  'w'
+#define ANALOG_WRITE   'x'
+#define READ_PIDIN     'i'
+#define READ_PIDOUT    'o'
+#define SOUND_BEEP     'f'
+#define CODE_VERSION   'v'
+
+#endif
+
+

arduinoCode/mobileBase/encoder_driver.h

@@ -0,0 +1,31 @@
+/**************************************************************
+Description: Encoder driver function definitions - by James Nugen
+Author: www.corvin.cn
+History: 20180209:init this file;
+*************************************************************/
+#ifndef __ENCODER_DRIVER_H__
+#define __ENCODER_DRIVER_H__
+
+//below can be changed, but should be PORTD pins;
+//A wheel encode pin
+#define ENC_A_PIN_A  20 //pin 20 -- interrupt 3   
+#define ENC_A_PIN_B  21 //pin 21 -- interrupt 2   
+
+//B wheel encode pin
+#define ENC_B_PIN_A  2  //pin 2 -- interrupt 0
+#define ENC_B_PIN_B  3  //pin 3 -- interrupt 1
+
+//C wheel encode pin
+#define ENC_C_PIN_A  18  //pin 18 -- interrupt 5
+#define ENC_C_PIN_B  19  //pin 19 -- interrupt 4 
+
+#define A_WHEEL      1
+#define B_WHEEL      2
+#define C_WHEEL      3
+
+void initEncoders(void);
+long readEncoder(int i);
+void resetEncoders(void);
+
+#endif
+

arduinoCode/mobileBase/encoder_driver.ino

@@ -0,0 +1,99 @@
+/**************************************************************
+Description:  Encoder definitions需要连接到arduinoMega2560的正确引脚上：
+   Encoder A: connect interrupt 0, 1--[pin 2, 3];
+   Encoder B: connect interrupt 2, 3--[pin 21, 20];
+   Encoder C: connect intterupt 4, 5--[pin 19, 18]
+Author: www.corvin.cn
+History: 20180209:init this file;
+**************************************************************/
+#include "motor_driver.h"
+#include "encoder_driver.h"
+
+static volatile long A_enc_cnt = 0L;
+static volatile long B_enc_cnt = 0L;
+static volatile long C_enc_cnt = 0L;
+
+/*init encoder connect pin, config ISR functions*/
+void initEncoders(void)
+{
+  pinMode(ENC_A_PIN_A, INPUT);
+  pinMode(ENC_A_PIN_B, INPUT);
+  attachInterrupt(3, encoderA_ISR, CHANGE);
+  attachInterrupt(2, encoderA_ISR, CHANGE);
+
+  pinMode(ENC_B_PIN_A, INPUT);
+  pinMode(ENC_B_PIN_B, INPUT);
+  attachInterrupt(0, encoderB_ISR, CHANGE);
+  attachInterrupt(1, encoderB_ISR, CHANGE);
+
+  pinMode(ENC_C_PIN_A, INPUT);
+  pinMode(ENC_C_PIN_B, INPUT);
+  attachInterrupt(5, encoderC_ISR, CHANGE);
+  attachInterrupt(4, encoderC_ISR, CHANGE);
+}
+
+/* Interrupt routine for A encoder, taking care of actual counting */
+void encoderA_ISR (void)
+{
+  if (directionWheel(A_WHEEL) == BACKWARDS)
+  {
+    A_enc_cnt--;
+  }
+  else 
+  {
+    A_enc_cnt++;
+  }
+}
+
+/* Interrupt routine for B encoder, taking care of actual counting */
+void encoderB_ISR (void) 
+{
+  if (directionWheel(B_WHEEL) == BACKWARDS)
+  {
+    B_enc_cnt--;
+  }
+  else 
+  {
+    B_enc_cnt++;
+  }
+}
+
+/* Interrupt routine for C encoder, taking care of actual counting */
+void encoderC_ISR (void) 
+{
+  if (directionWheel(C_WHEEL) == BACKWARDS)
+  {
+    C_enc_cnt--;
+  }
+  else 
+  {
+    C_enc_cnt++;
+  }
+}
+
+/* Wrap the encoder reading function */
+long readEncoder(int i) 
+{
+  if (i == A_WHEEL)
+  {
+    return A_enc_cnt;
+  }
+  else if (i == B_WHEEL)
+  {
+    return B_enc_cnt;
+  }
+  else
+  {
+    return C_enc_cnt;
+  }
+}
+
+/* Wrap the encoder count reset function */
+void resetEncoders(void) 
+{
+  A_enc_cnt = 0L;
+  B_enc_cnt = 0L;
+  C_enc_cnt = 0L;
+}
+
+

arduinoCode/mobileBase/mobileBase.ino

@@ -0,0 +1,318 @@
+/*********************************************************************
+  Description: A set of simple serial commands to control a omniWheel drive
+  robot and receive back sensor and odometry data. Default configuration
+  assumes use of an Arduino Mega 2560 + motor driver board. Edit the
+  readEncoder() and setMotorSpeed() wrapper functions if using different
+  motor controller or encoder method.
+  Author: www.corvin.cn
+  History: 20171129:init code;
+         20180209:增加代码版本号,定义初始版本号1.0,通过v命令来获取;
+         20180515:更新到1.1版本,整理代码架构,精简代码，使流程更清晰;
+*********************************************************************/
+#include "sound.h"
+#include "commands.h"
+#include "sensors.h"
+#include "motor_driver.h"
+#include "encoder_driver.h"
+#include "pid_controller.h"
+
+
+/******************** USER AREAR ********************/
+/* current code version */
+#define VERSION    1.1
+
+/* Stop the robot if it hasn't received a movement command
+  in this number of milliseconds */
+#define  AUTO_STOP_INTERVAL  260
+/******************** USER END **********************/
+
+/* Serial port baud rate */
+#define BAUDRATE   57600
+
+/* Run the PID loop at 30 times per second -Hz */
+#define PID_RATE   30
+
+/* Convert the rate into an interval */
+const int PID_INTERVAL = 1000 / PID_RATE;
+
+/* Track the next time we make a PID calculation */
+unsigned long nextPID = PID_INTERVAL;
+
+long lastMotorCommand = AUTO_STOP_INTERVAL;
+
+/* Variable initialization */
+// A pair of varibles to help parse serial commands (thanks Fergs)
+int arg   = 0;
+int index = 0;
+
+// Variable to hold an input character
+char chr;
+
+// Variable to hold the current single-character command
+char cmd;
+
+// Character arrays to hold the first,second,third arguments
+char argv1[48];
+char argv2[5];
+char argv3[5];
+
+// The arguments converted to integers
+int arg1 = 0;
+int arg2 = 0;
+int arg3 = 0;
+
+/* Clear the current command parameters */
+void resetCommand()
+{
+  cmd = '\0';
+  memset(argv1, 0, sizeof(argv1));
+  memset(argv2, 0, sizeof(argv2));
+  memset(argv3, 0, sizeof(argv3));
+
+  arg1  = 0;
+  arg2  = 0;
+  arg3  = 0;
+  arg   = 0;
+  index = 0;
+}
+
+/* Run a command.  Commands are defined in commands.h */
+int runCommand()
+{
+  int i   = 0;
+  char *p = argv1; //p pointer for update pid parameter
+  char *str;
+  int pid_args[12];
+
+  if (cmd != 'u') //cmd don't match update pid will convert
+  {
+    arg1 = atoi(argv1);
+    arg2 = atoi(argv2);
+    arg3 = atoi(argv3);
+  }
+  
+  switch (cmd)
+  {
+    case GET_BAUDRATE: //'b'
+      Serial.println(BAUDRATE);
+      break;
+
+    case ANALOG_READ:  //'a'
+      Serial.println(analogRead(arg1));
+      break;
+
+    case DIGITAL_READ: //'d'
+      Serial.println(digitalRead(arg1));
+      break;
+
+    case ANALOG_WRITE:
+      analogWrite(arg1, arg2);
+      Serial.println("OK");
+      break;
+
+    case DIGITAL_WRITE: //'w'
+      if (arg2 == 0)
+      {
+        digitalWrite(arg1, LOW);
+      }
+      else if (arg2 == 1)
+      {
+        digitalWrite(arg1, HIGH);
+      }
+      Serial.println("OK");
+      break;
+
+    case PIN_MODE:
+      if (arg2 == 0)
+      {
+        pinMode(arg1, INPUT);
+      }
+      else if (arg2 == 1)
+      {
+        pinMode(arg1, OUTPUT);
+      }
+      Serial.println("OK");
+      break;
+
+    case READ_ENCODERS:  //'e'
+      Serial.print(readEncoder(A_WHEEL));
+      Serial.print(" ");
+      Serial.print(readEncoder(B_WHEEL));
+      Serial.print(" ");
+      Serial.println(readEncoder(C_WHEEL));
+      break;
+
+    case RESET_ENCODERS: //'r'
+      resetEncoders();
+      resetPID();
+      Serial.println("OK");
+      break;
+
+    case MOTOR_SPEEDS: //'m'
+      lastMotorCommand = millis();  /* Reset the auto stop timer */
+      if (arg1 == 0 && arg2 == 0 && arg3 == 0)
+      {
+        setMotorSpeeds(0, 0, 0);
+        resetPID();
+        setMoveStatus(0);
+      }
+      else
+      {
+        setMoveStatus(1);
+      }
+      setWheelPIDTarget(arg1, arg2, arg3);
+      Serial.println("OK");
+      break;
+
+    case UPDATE_PID: //'u'
+      while ((str = strtok_r(p, ":", &p)) != '\0')
+      {
+        pid_args[i] = atoi(str);
+        i++;
+      }
+      updatePIDParam(A_WHEEL, pid_args[0], pid_args[1], pid_args[2],  pid_args[3]);
+      updatePIDParam(B_WHEEL, pid_args[4], pid_args[5], pid_args[6],  pid_args[7]);
+      updatePIDParam(C_WHEEL, pid_args[8], pid_args[9], pid_args[10], pid_args[11]);
+      Serial.println("OK");
+      break;
+
+    case SOUND_BEEP: //'f'
+      if (arg1 == BASE_POWERON_BEEP)
+      {
+        basePowerOnBeep();
+      }
+      else if (arg1 == BASE_POWEROFF_BEEP)
+      {
+        basePowerOffBeep();
+      }
+      Serial.println("OK");
+      break;
+
+    case READ_PIDIN:
+      Serial.print(readPidIn(A_WHEEL));
+      Serial.print(" ");
+      Serial.print(readPidIn(B_WHEEL));
+      Serial.print(" ");
+      Serial.println(readPidIn(C_WHEEL));
+      break;
+
+    case READ_PIDOUT:
+      Serial.print(readPidOut(A_WHEEL));
+      Serial.print(" ");
+      Serial.print(readPidOut(B_WHEEL));
+      Serial.print(" ");
+      Serial.println(readPidOut(C_WHEEL));
+      break;
+
+    case CODE_VERSION:
+      Serial.println(VERSION);
+      break;
+
+    default:
+      Serial.println("Invalid Command");
+      break;
+  }
+
+  return 0;
+}
+
+/* Setup function--runs once at startup. */
+void setup()
+{
+  Serial.begin(BAUDRATE);
+
+  initSensors();
+  initSoundPin();
+  initEncoders();
+  initMotorsPinMode();
+  resetPID();
+}
+
+/* Enter the main loop.  Read and parse input from the serial port
+   and run any valid commands. Run a PID calculation at the target
+   interval and check for auto-stop conditions.*/
+void loop()
+{
+  while (Serial.available() > 0)
+  {
+    chr = Serial.read();  //Read the next character
+
+    if (chr == 13)  //Terminate a command with a CR
+    {
+      if (arg == 1)
+      {
+        argv1[index] = '\0';
+      }
+      else if (arg == 2)
+      {
+        argv2[index] = '\0';
+      }
+      else if (arg == 3)
+      {
+        argv3[index] = '\0';
+      }
+      runCommand();
+      resetCommand();
+    }
+    else if (chr == ' ') // Use spaces to delimit parts of the command
+    {
+      // Step through the arguments
+      if (arg == 0)
+      {
+        arg = 1;
+      }
+      else if (arg == 1)
+      {
+        argv1[index] = '\0';
+        arg   = 2;
+        index = 0;
+      }
+      else if (arg == 2)
+      {
+        argv2[index] = '\0';
+        arg   = 3;
+        index = 0;
+      }
+      continue;
+    }
+    else // process single-letter command
+    {
+      if (arg == 0)
+      {
+        cmd = chr;  //The first arg is the single-letter command
+      }
+      else if (arg == 1)
+      {
+        // Subsequent arguments can be more than one character
+        argv1[index] = chr;
+        index++;
+      }
+      else if (arg == 2)
+      {
+        argv2[index] = chr;
+        index++;
+      }
+      else if (arg == 3)
+      {
+        argv3[index] = chr;
+        index++;
+      }
+    }
+  }//end while()
+
+  //run a PID calculation at the appropriate intervals
+  if (millis() > nextPID)
+  {
+    updatePID();
+    nextPID += PID_INTERVAL;
+  }
+
+  //Check to see if we have exceeded the auto-stop interval
+  if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL)
+  {
+    setMotorSpeeds(0, 0, 0); //stop motors
+    setMoveStatus(0);
+  }
+}
+
+

arduinoCode/mobileBase/motor_driver.h

@@ -0,0 +1,17 @@
+/***************************************************************
+Description: Motor driver function definitions - by James Nugen
+Author: www.corvin.cn
+History: 20180209: init this file;
+****************************************************************/
+#ifndef __MOTOR_DRIVER_H__
+#define __MOTOR_DRIVER_H__
+
+#define FORWARDS     true
+#define BACKWARDS    false
+
+void initMotorsPinMode(void);
+boolean directionWheel(int wheel);
+void setMotorSpeeds(int ASpeed, int BSpeed, int CSpeed);
+
+#endif
+

arduinoCode/mobileBase/motor_driver.ino

@@ -0,0 +1,139 @@
+/***************************************************************
+  Description: Motor driver definitions;
+  Author: www.corvin.cn
+  Hisotry: 20180209: init this file;
+*************************************************************/
+#include "motor_driver.h"
+
+/* Maximum PWM signal */
+#define MAX_PWM    255
+
+//three motors control pin define
+static const int A_IN1  = 26;
+static const int A_IN2  = 28;
+static const int A_PWM  = 4;   //A wheel pwm pin
+
+static const int B_IN1  = 30;
+static const int B_IN2  = 32;
+static const int B_PWM  = 6;   //B wheel pwm pin
+
+static const int C_IN1  = 24;
+static const int C_IN2  = 22;
+static const int C_PWM  = 5;   //C wheel pwm pin
+
+static boolean direcA = FORWARDS;
+static boolean direcB = FORWARDS;
+static boolean direcC = FORWARDS;
+
+/* Wrap the motor driver initialization,
+   set all the motor control pins to outputs **/
+void initMotorsPinMode(void)
+{
+  pinMode(A_IN1, OUTPUT);
+  pinMode(A_IN2, OUTPUT);
+  pinMode(A_PWM, OUTPUT);
+
+  pinMode(B_IN1, OUTPUT);
+  pinMode(B_IN2, OUTPUT);
+  pinMode(B_PWM, OUTPUT);
+
+  pinMode(C_IN1, OUTPUT);
+  pinMode(C_IN2, OUTPUT);
+  pinMode(C_PWM, OUTPUT);
+}
+
+/*
+   get wheel run direction
+*/
+boolean directionWheel(int wheel)
+{
+  if (wheel == A_WHEEL)
+  {
+    return direcA;
+  }
+  else if (wheel == B_WHEEL)
+  {
+    return direcB;
+  }
+  else
+  {
+    return direcC;
+  }
+}
+
+// A convenience function for setting both motor speeds
+void setMotorSpeeds(int ASpeed, int BSpeed, int CSpeed)
+{
+  if (ASpeed > MAX_PWM)
+  {
+    ASpeed = MAX_PWM;
+  }
+  else if (ASpeed < -MAX_PWM)
+  {
+    ASpeed = -1 * MAX_PWM;
+  }
+  
+  if (BSpeed > MAX_PWM)
+  {
+    BSpeed = MAX_PWM;
+  }
+  else if (BSpeed < -MAX_PWM)
+  {
+    BSpeed = -1 * MAX_PWM;
+  }
+  
+  if (CSpeed > MAX_PWM)
+  {
+    CSpeed = MAX_PWM;
+  }
+  else if (CSpeed < -MAX_PWM)
+  {
+    CSpeed = -1 * MAX_PWM;
+  }
+
+  if (ASpeed >= 0) //drive motor A
+  {
+    direcA = FORWARDS;
+    digitalWrite(A_IN1, LOW);
+    digitalWrite(A_IN2, HIGH);
+    analogWrite(A_PWM, ASpeed);
+  }
+  else if (ASpeed < 0)
+  {
+    direcA = BACKWARDS;
+    digitalWrite(A_IN1, HIGH);
+    digitalWrite(A_IN2, LOW);
+    analogWrite(A_PWM, -ASpeed);
+  }
+
+  if (BSpeed >= 0) //drive motor B
+  {
+    direcB = FORWARDS;
+    digitalWrite(B_IN1, LOW);
+    digitalWrite(B_IN2, HIGH);
+    analogWrite(B_PWM, BSpeed);
+  }
+  else if (BSpeed < 0)
+  {
+    direcB = BACKWARDS;
+    digitalWrite(B_IN1, HIGH);
+    digitalWrite(B_IN2, LOW);
+    analogWrite(B_PWM, -BSpeed);
+  }
+
+  if (CSpeed >= 0) //drive motor C
+  {
+    direcC = FORWARDS;
+    digitalWrite(C_IN1, LOW);
+    digitalWrite(C_IN2, HIGH);
+    analogWrite(C_PWM, CSpeed);
+  }
+  else if (CSpeed < 0)
+  {
+    direcC = BACKWARDS;
+    digitalWrite(C_IN1, HIGH);
+    digitalWrite(C_IN2, LOW);
+    analogWrite(C_PWM, -CSpeed);
+  }
+}
+

arduinoCode/mobileBase/pid_controller.h

@@ -0,0 +1,42 @@
+/************************************************************************
+Description:Functions and type-defs for PID control.
+   Taken mostly from Mike Ferguson's ArbotiX code which lives at:
+   http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
+Author: www.corvin.cn
+History: 20180209: init this file;
+**************************************************************************/
+#ifndef _PID_CONTROLLER_H_
+#define _PID_CONTROLLER_H_
+
+/* PID setpoint info For a Motor */
+typedef struct {
+  double TargetTicksPerFrame;    // target speed in ticks per frame
+  long Encoder;                  // encoder count
+  long PrevEnc;                  // last encoder count
+
+  /*
+    Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+  */
+  int PrevInput;                // last input
+
+  /*
+    Using integrated term (ITerm) instead of integrated error (Ierror),
+    to allow tuning changes,
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  int ITerm;                    //integrated term
+  long output;                  //last motor setting
+}SetPointInfo;
+
+void resetPID(void);
+void updatePID(void);
+long readPidIn(int wheel);
+long readPidOut(int wheel);
+void setMoveStatus(unsigned char state);
+unsigned char getMoveStatus(void);
+void setWheelPIDTarget(int value_A, int value_B, int value_C);
+void updatePIDParam(int index, int kp, int kd, int ki, int ko);
+
+#endif
+

arduinoCode/mobileBase/pid_controller.ino

@@ -0,0 +1,296 @@
+/************************************************************************
+  Description:PID控制器的代码实现。
+  Author: www.corvin.cn
+  History: 20180209: init this file;
+**************************************************************************/
+#include "pid_controller.h"
+#include "encoder_driver.h"
+
+static SetPointInfo AWheelPID, BWheelPID, CWheelPID;
+
+/*default PID Parameters */
+static int AWheel_Kp = 11;
+static int AWheel_Kd = 15;
+static int AWheel_Ki = 0;
+static int AWheel_Ko = 50;
+
+static int BWheel_Kp = 11;
+static int BWheel_Kd = 15;
+static int BWheel_Ki = 0;
+static int BWheel_Ko = 50;
+
+static int CWheel_Kp = 11;
+static int CWheel_Kd = 15;
+static int CWheel_Ki = 0;
+static int CWheel_Ko = 50;
+
+static unsigned char moving = 0; // is the base in motion?
+
+void setWheelPIDTarget(int value_A, int value_B, int value_C)
+{
+  AWheelPID.TargetTicksPerFrame = value_A;
+  BWheelPID.TargetTicksPerFrame = value_B;
+  CWheelPID.TargetTicksPerFrame = value_C;
+}
+
+unsigned char getMoveStatus(void)
+{
+  return moving;
+}
+
+void setMoveStatus(unsigned char state)
+{
+  moving = state;
+}
+
+void updatePIDParam(int index, int kp, int kd, int ki, int ko)
+{
+  switch (index)
+  {
+    case A_WHEEL:
+      AWheel_Kp = kp;
+      AWheel_Kd = kd;
+      AWheel_Ki = ki;
+      AWheel_Ko = ko;
+      break;
+
+    case B_WHEEL:
+      BWheel_Kp = kp;
+      BWheel_Kd = kd;
+      BWheel_Ki = ki;
+      BWheel_Ko = ko;
+      break;
+
+    case C_WHEEL:
+      CWheel_Kp = kp;
+      CWheel_Kd = kd;
+      CWheel_Ki = ki;
+      CWheel_Ko = ko;
+      break;
+
+    default:
+      break;
+  }
+}
+
+/*
+  Initialize PID variables to zero to prevent startup spikes
+  when turning PID on to start moving
+  In particular, assign both Encoder and PrevEnc the current encoder value
+  See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
+  Note that the assumption here is that PID is only turned on
+  when going from stop to moving, that's why we can init everything on zero.
+*/
+void resetPID(void)
+{
+  AWheelPID.TargetTicksPerFrame = 0.0;
+  AWheelPID.Encoder   = readEncoder(A_WHEEL);
+  AWheelPID.PrevEnc   = AWheelPID.Encoder;
+  AWheelPID.output    = 0L;
+  AWheelPID.PrevInput = 0;
+  AWheelPID.ITerm     = 0;
+
+  BWheelPID.TargetTicksPerFrame = 0.0;
+  BWheelPID.Encoder   = readEncoder(B_WHEEL);
+  BWheelPID.PrevEnc   = BWheelPID.Encoder;
+  BWheelPID.output    = 0L;
+  BWheelPID.PrevInput = 0;
+  BWheelPID.ITerm     = 0;
+
+  CWheelPID.TargetTicksPerFrame = 0.0;
+  CWheelPID.Encoder   = readEncoder(C_WHEEL);
+  CWheelPID.PrevEnc   = CWheelPID.Encoder;
+  CWheelPID.output    = 0L;
+  CWheelPID.PrevInput = 0;
+  CWheelPID.ITerm     = 0;
+}
+
+/* PID routine to compute the next A motor commands */
+void doAWheelPID(SetPointInfo *p)
+{
+  long Perror = 0L;
+  long output = 0L;
+  int input   = 0;
+
+  p->Encoder = readEncoder(A_WHEEL);
+  input      = p->Encoder - p->PrevEnc;
+  Perror     = p->TargetTicksPerFrame - input;
+
+  /*
+    Avoid derivative kick and allow tuning changes,
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  output = (AWheel_Kp * Perror - AWheel_Kd * (input - p->PrevInput) + p->ITerm) / AWheel_Ko;
+  p->PrevEnc = p->Encoder;  //save current encoder value to preEncoder
+
+  // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
+  output += p->output;
+  if (output >= MAX_PWM)
+  {
+    output = MAX_PWM;
+  }
+  else if (output <= -MAX_PWM)
+  {
+    output = -MAX_PWM;
+  }
+  else
+  {
+    /*
+      allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+    */
+    p->ITerm += AWheel_Ki * Perror;
+  }
+
+  p->output    = output;  //save current pid output for next pid
+  p->PrevInput = input;
+}
+
+/* PID routine to compute the next motor commands */
+void doBWheelPID(SetPointInfo *p)
+{
+  long Perror = 0L;
+  long output = 0L;
+  int input   = 0;
+
+  p->Encoder = readEncoder(B_WHEEL);
+  input      = p->Encoder - p->PrevEnc;
+  Perror     = p->TargetTicksPerFrame - input;
+
+  /*
+    Avoid derivative kick and allow tuning changes,
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  output = (BWheel_Kp * Perror - BWheel_Kd * (input - p->PrevInput) + p->ITerm) / BWheel_Ko;
+  p->PrevEnc = p->Encoder;
+
+  // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
+  output += p->output;
+  if (output >= MAX_PWM)
+  {
+    output = MAX_PWM;
+  }
+  else if (output <= -MAX_PWM)
+  {
+    output = -MAX_PWM;
+  }
+  else
+  {
+    /*
+      allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+    */
+    p->ITerm += BWheel_Ki * Perror;
+  }
+
+  p->output    = output;
+  p->PrevInput = input;
+}
+
+/* PID routine to compute the next motor commands */
+void doCWheelPID(SetPointInfo *p)
+{
+  long Perror = 0L;
+  long output = 0L;
+  int input   = 0;
+
+  p->Encoder = readEncoder(C_WHEEL);
+  input      = p->Encoder - p->PrevEnc;
+  Perror     = p->TargetTicksPerFrame - input;
+
+  /*
+    Avoid derivative kick and allow tuning changes,
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+    see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+  */
+  output = (CWheel_Kp * Perror - CWheel_Kd * (input - p->PrevInput) + p->ITerm) / CWheel_Ko;
+  p->PrevEnc = p->Encoder;
+
+  // Accumulate Integral error *or* Limit output. Stop accumulating when output saturates
+  output += p->output;
+  if (output >= MAX_PWM)
+  {
+    output = MAX_PWM;
+  }
+  else if (output <= -MAX_PWM)
+  {
+    output = -MAX_PWM;
+  }
+  else
+  {
+    /*
+      allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
+    */
+    p->ITerm += CWheel_Ki * Perror;
+  }
+
+  p->output    = output;
+  p->PrevInput = input;
+}
+
+/* Read the encoder values and call the PID routine */
+void updatePID(void)
+{
+  if (!moving) /* If we're not moving, resetPID() there is nothing more to do */
+  {
+    /*
+      Reset PIDs once, to prevent startup spikes,
+      see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
+      PrevInput is considered a good proxy to detect whether reset has already happened
+    */
+    if (AWheelPID.PrevInput != 0 || BWheelPID.PrevInput != 0 || CWheelPID.PrevInput != 0)
+    {
+      resetPID();
+    }
+  }
+  else
+  {
+    /* Compute PID update for each motor */
+    doAWheelPID(&AWheelPID);
+    doBWheelPID(&BWheelPID);
+    doCWheelPID(&CWheelPID);
+
+    /* Set the motor speeds accordingly */
+    setMotorSpeeds(AWheelPID.output, BWheelPID.output, CWheelPID.output);
+  }
+}
+
+long readPidIn(int wheel)
+{
+  long pidin = 0L;
+  if (wheel == A_WHEEL)
+  {
+    pidin = AWheelPID.PrevInput;
+  }
+  else if (wheel == B_WHEEL)
+  {
+    pidin = BWheelPID.PrevInput;
+  }
+  else
+  {
+    pidin = CWheelPID.PrevInput;
+  }
+
+  return pidin;
+}
+
+long readPidOut(int wheel)
+{
+  long pidout = 0L;
+  if (wheel == A_WHEEL)
+  {
+    pidout = AWheelPID.output;
+  }
+  else if (wheel == B_WHEEL)
+  {
+    pidout = BWheelPID.output;
+  }
+  else
+  {
+    pidout = CWheelPID.output;
+  }
+
+  return pidout;
+}
+
+

arduinoCode/mobileBase/sensors.h

@@ -0,0 +1,22 @@
+/*******************************************************************
+Description:Functions for various sensor types.
+Author: www.corvin.cn
+History: 20180209: init this file;
+********************************************************************/
+#ifndef __SENSORS_H__
+#define __SENSORS_H__
+
+static const int PowerContro_PIN = 52; //Mosfet Power controller sensor pin
+static const int alarm_powerControl_pin = 51;  //alarm light powercontrol sensor pin
+
+void initSensors()
+{
+  pinMode(PowerContro_PIN, OUTPUT);
+  digitalWrite(PowerContro_PIN, HIGH); //default enable Power controller
+
+  pinMode(alarm_powerControl_pin, OUTPUT);
+  digitalWrite(alarm_powerControl_pin, LOW); //default disable alarm light
+}
+
+#endif
+

arduinoCode/mobileBase/sound.h

@@ -0,0 +1,20 @@
+/**********************************************************
+Description: 下位机arduinoMega2560上的蜂鸣器模块头文件，
+  定义蜂鸣器连接的引脚，各种声音的宏定义。
+Author: www.corvin.cn
+History: 20180209: init this file;
+***********************************************************/
+
+#ifndef _SOUND_H_
+#define _SOUND_H_
+
+#define  SOUND_PIN      53
+
+#define  BASE_POWEROFF_BEEP  0
+#define  BASE_POWERON_BEEP   1
+
+void initSoundPin();
+void basePowerOnBeep();
+void basePowerOffBeep();
+
+#endif

arduinoCode/mobileBase/sound.ino

@@ -0,0 +1,32 @@
+/************************************************
+Description: 在arduino板上接蜂鸣器来生成各种声音反馈当前
+  arduinoMega2560的运行状态。
+Author: www.corvin.cn
+History: 20180209: init this file;
+*************************************************/
+
+void initSoundPin()
+{
+  pinMode(SOUND_PIN, OUTPUT);
+}
+
+void basePowerOnBeep()
+{
+  for (int i = 200; i <= 600; i++)
+  {
+    tone(SOUND_PIN, i); 
+    delay(3);
+  }
+  noTone(SOUND_PIN);
+}
+
+void basePowerOffBeep()
+{
+  for (int i = 600; i >= 200; i--)
+  {
+    tone(SOUND_PIN, i); 
+    delay(3);
+  }
+  noTone(SOUND_PIN);
+}
+

rosCode/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

rosCode/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

rosCode/src/android_communication/CMakeLists.txt

@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(android_app_communication)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  androidLogMsg.msg
+#   Message2.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES android_app_communication
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/android_app_communication.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/android_app_communication_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_android_app_communication.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/android_communication/msg/androidLogMsg.msg

@@ -0,0 +1 @@
+string message

rosCode/src/android_communication/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package>
+  <name>android_app_communication</name>
+  <version>0.0.0</version>
+  <description>The android_app_communication package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/android_app_communication</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rosCode/src/carebot_auto_gmapping/CHANGELOG.rst

@@ -0,0 +1,31 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package stdr_samples
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.3.2 (2017-02-06)
+------------------
+* Fix cmakes, no more cmake warnings
+* Include forgotten roslib dependency
+
+0.3.1 (2016-07-18)
+------------------
+
+0.3.0 (2016-07-18)
+------------------
+* Migrate to package format 2
+
+0.2.0 (2014-07-25)
+------------------
+
+0.1.3 (2014-03-25)
+------------------
+
+0.1.2 (2014-03-09)
+------------------
+
+0.1.1 (2014-02-10)
+------------------
+
+0.1.0 (2014-02-06)
+------------------
+* first public release for Hydro

rosCode/src/carebot_auto_gmapping/CMakeLists.txt

@@ -0,0 +1,45 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_auto_gmapping)
+
+find_package(catkin REQUIRED COMPONENTS
+  roslib
+  roscpp
+  tf
+  geometry_msgs
+  sensor_msgs
+  nav_msgs
+)
+
+set(CMAKE_BUILD_TYPE Release)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+  CATKIN_DEPENDS
+    roslib
+    roscpp
+    tf
+    geometry_msgs
+    sensor_msgs
+    nav_msgs
+)
+
+####################### Obstacle avoidance ##################################
+add_executable(carebot_automove_node
+  src/main.cpp
+  src/obstacle_avoidance.cpp)
+target_link_libraries(carebot_automove_node
+  ${catkin_LIBRARIES}
+)
+
+# Install excecutables
+install(TARGETS carebot_automove_node
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+

rosCode/src/carebot_auto_gmapping/include/obstacle_avoidance.h

@@ -0,0 +1,95 @@
+/******************************************************************************
+   STDR Simulator - Simple Two DImensional Robot Simulator
+   Copyright (C) 2013 STDR Simulator
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 3 of the License, or
+   (at your option) any later version.
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software Foundation,
+   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
+   
+   Authors : 
+   * Manos Tsardoulias, etsardou@gmail.com
+   * Aris Thallas, aris.thallas@gmail.com
+   * Chris Zalidis, zalidis@gmail.com 
+******************************************************************************/
+#ifndef STDR_OBSTACLE_AVOIDANCE_SAMPLE
+#define STDR_OBSTACLE_AVOIDANCE_SAMPLE
+
+#include <iostream>
+#include <cstdlib>
+#include <cmath>
+
+#include <ros/package.h>
+#include "ros/ros.h"
+
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/LaserScan.h>
+#include <sensor_msgs/Range.h>
+
+/**
+@namespace stdr_samples
+@brief The main namespace for STDR Samples
+**/ 
+namespace stdr_samples
+{
+  /**
+  @class ObstacleAvoidance
+  @brief Performs obstacle avoidance to a single robot
+  **/ 
+  class ObstacleAvoidance
+  {
+    private:
+      
+      //!< The ros laser scan msg
+      sensor_msgs::LaserScan scan_;
+      
+      //!< Subscriber for the ros laser msg
+      ros::Subscriber subscriber_;
+      
+      //!< The ROS node handle
+      ros::NodeHandle n_;
+      
+      //!< The laser topic
+      std::string laser_topic_;
+      
+      //!< The speeds topic
+      std::string speeds_topic_;
+      
+      //!< The twist publisher
+      ros::Publisher cmd_vel_pub_;
+      
+    public:
+    
+      /**
+      @brief Default contructor
+      @param argc [int] Number of input arguments
+      @param argv [char **] Input arguments
+      @return void
+      **/
+      ObstacleAvoidance(std::string, std::string);
+      
+      /**
+      @brief Default destructor
+      @return void
+      **/
+      ~ObstacleAvoidance(void);
+      
+      /**
+      @brief Callback for the ros laser message
+      @param msg [const sensor_msgs::LaserScan&] The new laser scan message
+      @return void
+      **/
+      void callback(const sensor_msgs::LaserScan& msg);
+      
+  };
+}
+
+#endif

rosCode/src/carebot_auto_gmapping/launch/ls01d_lidar_gmapping.launch

@@ -0,0 +1,10 @@
+<launch>
+  <!-- fist startup ls01d lidar -->
+  <include file="$(find ls01d_lidar)/slam_launch/gmapping_ls01d_lidar.launch" />
+  
+  <node name="carebot_automove_node" pkg="carebot_auto_gmapping" type="carebot_automove_node" output="screen">
+      <param name="cmd_topic" value="/cmd_vel" />  
+      <param name="laser_topic" value="/scan" />  
+  </node>
+
+</launch>

rosCode/src/carebot_auto_gmapping/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>carebot_auto_gmapping</name>
+  <version>0.3.2</version>
+  <description>Provides carebot_automove to gmapping</description>
+
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
+
+  <license>GPLv3</license>
+
+  <author>Manos Tsardoulias, Chris Zalidis, Aris Thallas</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>roslib</depend>
+  <depend>roscpp</depend>
+  <depend>tf</depend>
+  <depend>sensor_msgs</depend>
+  <depend>stdr_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+  <export>
+  </export>
+
+</package>
+

rosCode/src/carebot_auto_gmapping/src/main.cpp

@@ -0,0 +1,45 @@
+/******************************************************************************
+   STDR Simulator - Simple Two DImensional Robot Simulator
+   Copyright (C) 2013 STDR Simulator
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 3 of the License, or
+   (at your option) any later version.
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software Foundation,
+   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
+   
+   Authors : 
+   * Manos Tsardoulias, etsardou@gmail.com
+   * Aris Thallas, aris.thallas@gmail.com
+   * Chris Zalidis, zalidis@gmail.com 
+******************************************************************************/
+# include "obstacle_avoidance.h"
+
+/**
+@brief The main node function
+@param argc [int] Number of input arguments
+@param argv [char] The input arguments
+@return int : 0 for success
+**/
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "carebot_automove_node");
+  ros::NodeHandle ndHandle;
+
+  std::string cmd_topic = "/cmd_vel";
+  std::string laser_topic = "/scan";
+  
+  ros::param::get("~cmd_topic", cmd_topic);
+  ros::param::get("~laser_topic", laser_topic);
+  
+  stdr_samples::ObstacleAvoidance obj(cmd_topic, laser_topic);
+
+  ros::spin();
+  return 0;
+}
+

rosCode/src/carebot_auto_gmapping/src/obstacle_avoidance.cpp

@@ -0,0 +1,98 @@
+/******************************************************************************
+   STDR Simulator - Simple Two DImensional Robot Simulator
+   Copyright (C) 2013 STDR Simulator
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation; either version 3 of the License, or
+   (at your option) any later version.
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+   You should have received a copy of the GNU General Public License
+   along with this program; if not, write to the Free Software Foundation,
+   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
+   
+   Authors : 
+   * Manos Tsardoulias, etsardou@gmail.com
+   * Aris Thallas, aris.thallas@gmail.com
+   * Chris Zalidis, zalidis@gmail.com 
+******************************************************************************/
+# include "obstacle_avoidance.h"
+
+/**
+@namespace stdr_samples
+@brief The main namespace for STDR Samples
+**/ 
+namespace stdr_samples
+{
+  /**
+  @brief Default contructor
+  @param argc [int] Number of input arguments
+  @param argv [char **] Input arguments
+  @return void
+  **/
+  ObstacleAvoidance::ObstacleAvoidance(std::string cmd_topic, std::string laser_topic)
+  {
+    speeds_topic_ = cmd_topic;
+    laser_topic_  = laser_topic;
+      
+    subscriber_ = n_.subscribe(
+      laser_topic_.c_str(), 
+      1, 
+      &ObstacleAvoidance::callback,
+      this);
+      
+    cmd_vel_pub_ = n_.advertise<geometry_msgs::Twist>(speeds_topic_.c_str(), 1);
+  }
+  
+  /**
+  @brief Default destructor
+  @return void
+  **/
+  ObstacleAvoidance::~ObstacleAvoidance(void)
+  {
+    
+  }
+  
+  /**
+  @brief Callback for the ros laser message
+  @param msg [const sensor_msgs::LaserScan&] The new laser scan message
+  @return void
+  **/
+  void ObstacleAvoidance::callback(const sensor_msgs::LaserScan& msg)
+  {
+    scan_ = msg;
+    float linear = 0, rotational = 0;
+
+    for(unsigned int i = 0 ; i < scan_.ranges.size() ; i++)
+    {
+      float real_dist = scan_.ranges[i];
+      linear -= cos(scan_.angle_min + i * scan_.angle_increment) 
+                   / (1.0 + real_dist * real_dist);
+      rotational -= sin(scan_.angle_min + i * scan_.angle_increment) 
+                    / (1.0 + real_dist * real_dist);
+    }
+    
+    linear /= scan_.ranges.size();
+    rotational /= scan_.ranges.size();
+    ROS_WARN("%f %f",linear,rotational);
+    
+    if(linear > 0.1)
+    {
+      linear = 0.1;
+    }
+    else if(linear < -0.1)
+    {
+      linear = -0.1;
+    }
+
+    geometry_msgs::Twist cmd;
+    cmd.linear.x  = 0.1 + linear;
+    cmd.angular.z = rotational;
+    cmd_vel_pub_.publish(cmd);
+  }
+
+}
+
+

rosCode/src/carebot_bringup/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_bringup)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES bringup
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/bringup.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/bringup_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_bringup.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/carebot_bringup/launch/carebot_bringup.launch

@@ -0,0 +1,37 @@
+<!--
+  Copyright: 2016-2018(c) ROS小课堂
+  Author: www.corvin.cn
+  Description:carebot启动时最先需要启动的launch文件，主要加载各基础功能模块启动。
+    first startup urdf description file,then startup arduino mobilebase,then
+    startup imu module(gy-951), last startup robot_pose_ekf to pose estimate.
+  History:
+    20180116:init this file.
+-->
+<launch>
+    <!-- (1) startup robot urdf description -->
+    <include file="$(find carebot_description)/launch/carebot_description.launch" />
+
+    <!-- (2) startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+    <!-- (3) startup IMU node launch-->
+    <include file="$(find razor_imu_9dof)/launch/razor-pub.launch" />
+
+    <!-- (4) startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+        <param name="output_frame" value="odom_combined"/>
+        <param name="base_footprint_frame" value="base_footprint"/>
+        <param name="freq" value="15.0"/>
+        <param name="sensor_timeout" value="1.0"/>  
+        <param name="odom_used" value="true"/>
+        <param name="imu_used" value="true"/>
+        <param name="vo_used" value="false"/>
+        <param name="debug" value="false"/>
+
+        <remap from="imu_data" to="imu" />
+   </node>
+
+   <!-- (5) startup convert /odom_combined to /odom_ekf can view in rviz-->
+    <include file="$(find carebot_bringup)/launch/odom_ekf.launch"/>
+</launch>
+

rosCode/src/carebot_bringup/launch/odom_ekf.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="carebot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
+    <remap from="output" to="/odom_ekf"/>
+  </node>
+</launch>

rosCode/src/carebot_bringup/launch/odom_ekf.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+""" odom_ekf.py - Version 1.1 2013-12-20
+
+    Republish the /robot_pose_ekf/odom_combined topic which is of type 
+    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
+    type nav_msgs/Odometry so we can view it in RViz.
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.5
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+      
+"""
+
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
+
+class OdomEKF():
+    def __init__(self):
+        # Give the node a name
+        rospy.init_node('odom_ekf_node', anonymous=False)
+
+        # Publisher of type nav_msgs/Odometry
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
+        
+        # Wait for the /odom_combined topic to become available
+        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+        
+        # Subscribe to the /odom_combined topic
+        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        
+        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+        
+    def pub_ekf_odom(self, msg):
+        odom = Odometry()
+        odom.header = msg.header
+        odom.header.frame_id = '/odom_combined'
+        odom.child_frame_id = 'base_footprint'
+        odom.pose = msg.pose
+        
+        self.ekf_pub.publish(odom)
+        
+if __name__ == '__main__':
+    try:
+        OdomEKF()
+        rospy.spin()
+    except:
+        pass
+        
+
+        

rosCode/src/carebot_bringup/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>carebot_bringup</name>
+  <version>0.0.0</version>
+  <description>The carebot_bringup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bringup</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rosCode/src/carebot_bringup/scripts/config_env.sh

@@ -0,0 +1,31 @@
+#!/bin/bash
+
+#################################################
+# Author: www.corvin.cn
+#################################################
+# Description: 执行该脚本就可以配置好运行环境,
+#   省去自己单独配置的麻烦过程。当编译完成源码后 
+#   就需要执行该脚本,该脚本只需要执行一次即可。
+#################################################
+# History:
+#    20180126:init this file.
+#
+#################################################
+
+CURRENT_PATH=$(dirname $(readlink -f "$0"))
+OMMIT_PATH="src/carebot_bringup/scripts"
+WORKSPACE_PATH=${CURRENT_PATH%${OMMIT_PATH}}
+green="\e[32;1m"
+normal="\e[0m"
+
+echo -e "${green}Get project workSpace path: ${WORKSPACE_PATH} ${normal}"
+
+echo -e "\n${green} 0x00: set source project's devel/setup.bash to .bashrc file${normal}"
+cd ${WORKSPACE_PATH}
+source devel/setup.bash
+echo "#config omniWheelCareRobot project env">>~/.bashrc
+echo "source ${WORKSPACE_PATH}devel/setup.bash">>~/.bashrc
+
+
+exit 0
+

rosCode/src/carebot_bringup/startup/carebot.service

@@ -0,0 +1,13 @@
+[Unit]
+Description=carebot startup created by corvin 
+
+[Service]
+Restart=always
+RestartSec=30
+ExecStart=/usr/sbin/carebot_start
+ExecStop=/usr/sbin/carebot_stop
+ExecRestart=/usr/sbin/carebot_restart
+
+[Install]
+WantedBy=multi-user.target
+

rosCode/src/carebot_bringup/startup/carebot_restart

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+log_file=/home/$USER/.carebot/carebot-upstart.log
+DATE=`date`
+echo "$DATE: carebot-stop" >> $log_file
+
+/usr/sbin/carebot_stop
+sleep 2
+/usr/sbin/carebot_start 
+

rosCode/src/carebot_bringup/startup/carebot_start

@@ -0,0 +1,21 @@
+#!/bin/bash
+
+DATE=`date`
+log_file=/home/corvin/.carebot/carebot-upstart.log
+source /opt/ros/kinetic/setup.bash
+source /home/corvin/omniWheelCareRobot/rosCode/devel/setup.bash
+
+interface=wlan0
+echo "$DATE: carebot_start on interface $interface" >> $log_file
+
+sleep 3
+export ROS_IP=`ifconfig $interface | grep -o 'inet addr:[^ ]*' | cut -d: -f2`
+echo "$DATE: carebot_start setting ROS_IP=$ROS_IP" >> $log_file
+
+if [ "$ROS_IP" = "" ]; then
+    echo "$DATE: carebot_start can't run with empty ROS_IP." >> $log_file
+    exit 1
+fi
+
+roslaunch carebot_bringup carebot_bringup.launch
+

rosCode/src/carebot_bringup/startup/carebot_stop

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+log_file=/home/$USER/.carebot/carebot-upstart.log
+DATE=`date`
+echo "$DATE: carebot-stop" >> $log_file
+
+rosservice call /stop_motor
+
+for i in $( rosnode list ); do
+    rosnode kill $i;
+done
+
+killall roslaunch
+

rosCode/src/carebot_bringup/startup/readme.txt

@@ -0,0 +1,35 @@
+/**************************************************************/
+/* Author: www.corvin.cn                                      */
+/**************************************************************/
+/* Description: 该目录下文件都是为了保证carebot在刚加电开机   */
+/*   后可以直接进入工作状态，即新建启动的service这样可以跟    */
+/*   随系统来一起启动需要启动的launch文件。                   */
+/*   只需要按照下面所叙述的操作步骤执行即可完成,注意的是      */
+/*   该操作只在树莓派3B板上测试通过。                         */
+/**************************************************************/
+/* History:                                                   */
+/*   20180116:init this file.                                 */
+/*                                                            */
+/**************************************************************/
+
+1.设置开机启动服务,直接在终端中执行当前目录下的setup.sh脚本即可，命令如下：
+./setup.bash
+如果提示输入密码,直接输入corvin即可。
+
+2.检查是否设置了开机启动服务,通过以下命令来查看carebot服务是否设置成功，
+如果提示enabled，则说明设置成功了,直接在终端中执行命令即可。
+systemctl is-enabled carebot.service
+
+3.在调试中如果需要临时停止该启动服务，则在终端中执行以下命令:
+sudo systemctl stop carebot.service
+
+4.如果想再次临时启动该服务，在终端中执行如下命令:
+sudo systemctl start carebot.service
+
+5.如果不想在开机时启动该服务了，则通过下面命令来取消开机自启动:
+sudo systemctl disable carebot.service
+
+6.如果又想在开机时自动启动该服务了，则执行以下命令即可:
+sudo systemctl enable carebot.service
+
+

rosCode/src/carebot_bringup/startup/setup.sh

@@ -0,0 +1,27 @@
+#!/bin/bash
+
+dirPath="/home/$USER/.carebot/"
+
+#create carebot config folder to save log
+if [ -d "$dirPath" ]
+then
+  echo "Already exist target folder,now delete old create new foler..."
+  rm -rf "$dirPath"
+  mkdir "$dirPath" 
+else
+  mkdir "$dirPath" 
+fi
+
+sudo cp carebot_start /usr/sbin/
+sudo chmod +x /usr/sbin/carebot_start
+
+sudo cp carebot_stop /usr/sbin/
+sudo chmod +x /usr/sbin/carebot_stop
+
+sudo cp carebot_restart /usr/sbin/
+sudo chmod +x /usr/sbin/carebot_restart
+
+sudo cp carebot.service /lib/systemd/system/
+
+sudo systemctl enable carebot.service
+

rosCode/src/carebot_calibration/CMakeLists.txt

@@ -0,0 +1,198 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_calibration)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES carebot_calibration
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/carebot_calibration.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/carebot_calibration_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_carebot_calibration.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/carebot_calibration/config/angular_calibrate_params.yaml

@@ -0,0 +1,34 @@
+#######################################################################
+# Copyright: 2016-2018(c) ROS小课堂 www.corvin.cn
+#######################################################################
+# Author: corvin
+#######################################################################
+# Description:
+#  该参数文件是为校准小车的角速度而设置，大家可以根据需要酌情来修改各
+#  个参数，各参数功能介绍分别是：
+#  test_circle:测试小车需要转动的圈数,默认2,正转2圈。
+#  angular_speed:小车转动时的角速度多大,default 0.2rad/s。
+#  tolerance_angle:在测试转动时可以容忍的误差,默认0.05弧度。
+#  angular_scale:校准小车在转动时的精度,根据小车实际转动的角度除以规定
+#    的角度得到结果作为新的angular_scale值，再一次运行，根据这次的误差
+#    乘以angular_scale作为这次的新angular_scale，一直测试直到最终实际
+#    角度达到预期。
+#  check_odom_rate:while循环查询转动角度的频率.
+#  cmd_topic:控制小车移动的话题.
+#  base_frame:小车的基坐标系。
+#  odom_frame:小车里程计的坐标系。
+#######################################################################
+# History:
+#  20180111:初始化该参数文件。
+#  20180206:增加angular_scale配置参数。
+#  20180425:将test_angle测试角度改为test_circle,直接指定测试几圈,
+#      将角速度从0.5rad/s改为0.2rad/s,tolerance_angel从0.03->0.05.
+#######################################################################
+test_circle: 2 #test run circle
+angular_speed: 0.2  #default 0.2rad/s
+tolerance_angle: 0.04 #tolerance radians
+angular_scale: 1.00
+check_odom_rate: 15 #check odom rate
+cmd_topic: /cmd_vel
+base_frame: base_footprint
+odom_frame: odom

rosCode/src/carebot_calibration/config/linear_calibrate_params.yaml

@@ -0,0 +1,35 @@
+#######################################################################
+# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+#######################################################################
+# Author: corvin
+#######################################################################
+# Description:
+#  该参数文件是为校准小车的线速度而设置，大家可以根据需要酌情来修改各
+#  个参数，各参数功能介绍分别是：
+#  test_distance:测试小车需要移动的距离,默认是2米。
+#  linear_speed:小车移动时速度多大。
+#  tolerance_linear:在测试移动时可以容忍的误差。
+#  linear_scale:小车在移动中线速度精度,根据小车实际走的距离去除以
+#    test_distance得到的数据就是,如果小车走的还是不准确的话就继续
+#    运行一次,根据实际走的距离乘以当前的linear_scale作为新的linear_scale.
+#  check_rate:根据小车底盘发布里程计坐标的频率来设置检查的频率.
+#  cmd_topic:小车底盘订阅控制其移动的话题名称.
+#  base_frame:小车底盘的基坐标系.
+#  odom_frame:小车里程计的坐标系,我们需要查询这两个坐标系之间的距离来
+#    判断我们的小车是否移动到了指定的距离.
+#  我们最终标定完以后,只需要记下这个linear_scale参数就可以了.我们需要将
+#  该参数配置在控制我们小车底盘移动的上位机代码中.这样我们的小车以后在
+#  移动时距离精度就会很好了.
+#######################################################################
+# History:
+#  20180111:初始化该参数文件.
+#  20180911:增加check_rate和cmd_topic两个参数.
+#######################################################################
+test_distance: 2.0  # m
+linear_speed: 0.17  # m/s
+tolerance_linear: 0.005  # 0.5cm
+linear_scale: 1.0
+check_rate: 16
+cmd_topic: /cmd_vel
+base_frame: base_footprint
+odom_frame: odom

rosCode/src/carebot_calibration/config/rviz_config.rviz

@@ -0,0 +1,173 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 0
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /TF1/Frames1
+      Splitter Ratio: 0.459227
+    Tree Height: 437
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.2
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 191; 220; 182
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 8
+        Y: 8
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 30
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.3
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_footprint:
+          Value: true
+        base_imu_link:
+          Value: false
+        base_link:
+          Value: false
+        laser:
+          Value: false
+        odom:
+          Value: true
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom:
+          base_footprint:
+            base_link:
+              base_imu_link:
+                laser:
+                  {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2.43991
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.16892
+        Y: 0.333107
+        Z: -0.00418486
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.674798
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.72363
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 998
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df0000018500000127fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000035f000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001ad000001d70000000000000000fb0000000c00430061006d006500720061010000036e000000160000000000000000fb0000000a0049006d00610067006501000001c4000001c30000000000000000000000010000010f0000035cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000035c000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006900000003bfc0100000002fb0000000800540069006d0065010000000000000690000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000006900000035f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1680
+  X: 53
+  Y: 14

rosCode/src/carebot_calibration/launch/calibrate_mobilebase_angular.launch

@@ -0,0 +1,21 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    进行小车底盘角速度标定的launch启动文件,首先启动小车的urdf描述启动文件,用于在
+    rviz中进行查看标定过程.然后启动控制底盘移动的launch文件,最后启动角速度标定的
+    python脚本,并且启动时要加载相应配置参数.
+  History:
+    20180425: init this file.
+-->
+<launch>
+  <!--startup robot urdf description file-->
+  <include file="$(find carebot_description)/launch/carebot_description.launch" />
+
+  <!--startup mobilebase arduino launch -->
+  <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+  <node pkg="carebot_calibration" type="calibrate_mobilebase_angular.py" name="calibrate_angular_node" output="screen" >
+    <rosparam file="$(find carebot_calibration)/config/angular_calibrate_params.yaml" command="load" />
+  </node>
+</launch>

rosCode/src/carebot_calibration/launch/calibrate_mobilebase_linear.launch

@@ -0,0 +1,28 @@
+<!--
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:该launch文件是用于对三轮全向移动小车底盘的线速度进行标定.通过设置往前移动的
+   指定距离,然后实地测量小车底盘的实际移动距离与设定的移动距离的误差.根据该误差修正配置
+   参数,从而可以减小该误差,使小车底盘移动距离更准确,为后面的自动导航做好准备工作.
+   该启动文件主要启动流程如下:
+   1.启动小车的urdf文件,这样方便在RViz中查看整个标定过程.
+   2.启动小车控制底盘移动的上位机程序.
+   3.启动标定程序,从配置文件中读取测试移动距离、移动速度、控制移动的话题等配置信息.
+     根据该信息来控制小车移动,测试完成后,需要拿出米尺来测量小车的实际移动距离与配置
+     的移动距离的差值,然后标定程序会根据测量值来修正配置参数,直到实际的移动距离与配
+     置的移动距离之间的误差在可容忍范围内即可.最后的linear_scale参数就是最终的标定参数,
+     到这里线速度标定过程完毕,最后我们将linear_scale写入到控制小车移动的上位机配置文件中即可.
+ History:
+   20180907:initial this comment.
+-->
+<launch>
+    <!--startup robot urdf description -->
+    <include file="$(find carebot_description)/launch/carebot_description.launch"/>
+
+    <!--startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+    <node pkg="carebot_calibration" type="calibrate_mobilebase_linear.py" name="calibrate_mobilebase_linear" output="screen">
+        <rosparam file="$(find carebot_calibration)/config/linear_calibrate_params.yaml" command="load" />
+    </node>
+</launch>

rosCode/src/carebot_calibration/launch/rviz_display.launch

@@ -0,0 +1,11 @@
+<!--
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:可以在小车的线速度和角速度标定过程中，在rviz中查看整个过程.
+  History:
+    20180915: initial this file.
+-->
+<launch>
+  <!-- startup rviz with config file -->
+  <node name="rviz" type="rviz" pkg="rviz" args="-d $(find carebot_calibration)/config/rviz_config.rviz" />
+</launch>

rosCode/src/carebot_calibration/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>carebot_calibration</name>
+  <version>0.0.0</version>
+  <description>The carebot_calibration package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/carebot_calibration</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rosCode/src/carebot_calibration/src/calibrate_mobilebase_angular.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+  Copyright(c):2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    三轮全向移动小车底盘角速度标定的源码文件.
+  History:
+    20180425: init this file.
+    20180919:增加结束时自动计算angular_scale的功能.
+"""
+import tf
+import PyKDL
+import rospy
+from math import radians, pi
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Twist, Quaternion
+
+
+class CalibrateAngular():
+    def __init__(self):
+        rospy.init_node('calibrate_angular_node', anonymous=False)
+        rospy.on_shutdown(self.shutdown)
+
+        #from config file get param
+        self.get_param()
+
+        # How fast will we check the odometry values?
+        check_rate = rospy.Rate(self.rate)
+
+        # Initialize the tf listener
+        self.tf_listener = tf.TransformListener()
+        rospy.sleep(2)
+
+        # Make sure we see the odom and base frames
+        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(10.0))
+
+        #initial parameters
+        self.init_param()
+        self.last_angle = 0
+
+        # config move cmd msg
+        move_cmd = Twist()
+        move_cmd.angular.z = self.speed
+
+        while not rospy.is_shutdown():
+            if self.rest_angle > self.tolerance:
+                # Get the current rotation angle from tf
+                odom_angle = self.get_odom_angle()
+                rospy.loginfo("current rotation angle: " + str(odom_angle))
+
+                # Compute how far we have gone since the last measurement
+                delta_angle = self.angular_scale*self.normalize_angle(odom_angle - self.last_angle)
+                self.turn_angle += abs(delta_angle)
+
+                # Compute the rest angle
+                self.rest_angle = self.test_angle - self.turn_angle
+                rospy.loginfo("-->rest_angle: " + str(self.rest_angle))
+
+                # Store the current angle for the next comparison
+                self.last_angle = odom_angle
+
+                self.cmd_vel.publish(move_cmd)
+                check_rate.sleep()
+            else: #end test
+                self.print_result()
+
+    def get_param(self):
+        try:
+            self.rate = rospy.get_param("~check_odom_rate", 15)
+            self.circle_cnt = rospy.get_param("~test_circle", 2)
+            self.test_angle = eval('self.circle_cnt*2*pi')
+
+            self.speed = rospy.get_param("~angular_speed", 0.2) #radians speed
+            self.tolerance = rospy.get_param("~tolerance_angle", 0.05)
+            self.angular_scale = rospy.get_param("~angular_scale", 1.00)
+
+            cmd_topic = rospy.get_param("~cmd_topic", '/cmd_vel')
+            self.cmd_vel = rospy.Publisher(cmd_topic, Twist, queue_size=10)
+
+            # Get base frame and odom frame
+            self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
+            self.odom_frame = rospy.get_param('~odom_frame', '/odom')
+        except:
+            rospy.logerr("ERROR: Get config param error from yaml file...")
+
+    def init_param(self):
+        self.start_time = rospy.get_time()
+        self.rest_angle = self.test_angle
+        self.turn_angle = 0.0
+
+    def print_result(self):
+        end_time = rospy.get_time() #get test end time
+        rospy.logwarn("---------------------------------")
+        rospy.logwarn("---Angular Calibrate Completed---")
+        rospy.logwarn("---------------------------------")
+        rospy.logwarn("Test angle:" + str(self.test_angle))
+        rospy.logwarn("Test Time:"  + str(end_time-self.start_time))
+        rospy.logwarn("Test Speed:" + str(self.test_angle/(end_time-self.start_time)))
+        rospy.logwarn("Input Speed:" + str(self.speed))
+        rospy.logwarn("Angular Scale:" + str(self.angular_scale))
+        actual_degree = input("Please input actual turn degree:")
+        angular_scale = float(radians(actual_degree)/self.test_angle)
+        self.angular_scale *= angular_scale
+        rospy.logwarn("Now get new angular_scale:" + str(self.angular_scale))
+        self.init_param()
+
+    def normalize_angle(self, angle):
+        res = angle
+        while res > pi:
+            res -= 2.0*pi
+        while res < -pi:
+            res += 2.0*pi
+        return res
+
+    def quat_to_angle(self, quat):
+        rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
+        return rot.GetRPY()[2]
+
+    # Get the current transform between the odom and base frames
+    def get_odom_angle(self):
+        try:
+            (position, quat) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
+        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
+            rospy.logerr("get_odom_angle: TF Exception !")
+            return
+        # Convert the rotation from a quaternion to an Euler angle
+        return self.quat_to_angle(Quaternion(*quat))
+
+    def shutdown(self):
+        rospy.logwarn("shutdown():Stopping the robot...")
+        self.cmd_vel.publish(Twist())
+        rospy.sleep(1)
+
+if __name__ == '__main__':
+    try:
+        CalibrateAngular()
+        rospy.spin()
+    except:
+        rospy.logerr("Error: Calibration terminated.")
+

rosCode/src/carebot_calibration/src/calibrate_mobilebase_linear.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+
+"""
+ Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+  该源码文件是标定三轮全向移动小车的底盘线速度的代码,标定的主要过程
+  就是根据设定的移动距离,然后向小车的移动话题中发布移动速度.在此时,
+  不断的监听小车自身的基坐标系与odom坐标系之间的距离.当检测到两坐标
+  系之间的距离已经小于容忍范围内,说明小车已经到了指定的移动距离.
+  此时就需要测量小车的实际移动距离跟坐标系间检测的距离是否一样,
+  两者之间的误差是否可以接受,如果不接受就需要修改里程计的修正值.
+ History:
+  20180907: initial this file.
+"""
+import tf
+import rospy
+from math import copysign, sqrt, pow
+from geometry_msgs.msg import Twist, Point
+
+class CalibrateLinear():
+    def __init__(self):
+        rospy.init_node('calibrate_linear_node', anonymous=False)
+
+        #execute a shutdown function when terminating the script
+        rospy.on_shutdown(self.shutdown)
+
+        self.test_distance = rospy.get_param("~test_distance", 2.0)
+        self.speed     = rospy.get_param("~linear_speed", 0.17)
+        self.tolerance = rospy.get_param("~tolerance_linear", 0.005)
+        self.odom_linear_scale = rospy.get_param("~linear_scale", 1.000)
+        self.rate  = rospy.get_param("~check_rate", 15)
+        check_rate = rospy.Rate(self.rate)
+        self.start_test = True  #default when startup run calibrate
+
+        #Publisher to control the robot's speed
+        self.cmd_topic = rospy.get_param("~cmd_topic", '/cmd_vel')
+        self.cmd_vel   = rospy.Publisher(self.cmd_topic, Twist, queue_size=5)
+
+        #The base frame is base_footprint for the robot,odom_frame is odom
+        self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
+        self.odom_frame = rospy.get_param('~odom_frame', '/odom')
+
+        #initialize the tf listener and wait
+        self.tf_listener = tf.TransformListener()
+        rospy.sleep(2)
+
+        #make sure we see the odom and base frames
+        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(20.0))
+        self.position = Point()
+
+        #get the starting position from the tf between the odom and base frames
+        self.position = self.get_position()
+        self.x_start  = self.position.x
+        self.y_start  = self.position.y
+
+        #print start calibrate summary info
+        self.print_summary()
+
+        while not rospy.is_shutdown():
+            #get the starting position from the tf between the odom and base frames
+            self.position = self.get_position()
+            check_rate.sleep() #sleep for while loop
+
+            if self.start_test:
+                #compute the euclidean distance from the target point
+                distance = sqrt(pow((self.position.x - self.x_start), 2) +
+                                pow((self.position.y - self.y_start), 2))
+
+                #correct the estimate distance by the correction factor
+                distance *= self.odom_linear_scale
+                error = self.test_distance - distance
+                rospy.loginfo("-->rest_distance: " + str(error))
+
+                move_cmd = Twist()
+                if error < self.tolerance:
+                    self.start_test = False
+                    self.cmd_vel.publish(Twist())  #stop the robot
+                    rospy.logwarn("Now stop move robot !")
+                else:
+                    move_cmd.linear.x = self.speed
+                    self.cmd_vel.publish(move_cmd) #continue move
+            else:  #end test
+                rospy.logwarn("-> linear_scale: " + str(self.odom_linear_scale))
+                actual_dist = input("Please input actual distance:")
+                linear_scale_error = float(actual_dist)/self.test_distance
+                self.odom_linear_scale *= linear_scale_error
+                rospy.logwarn("Now get new linear_scale: " + str(self.odom_linear_scale))
+                self.print_summary()
+                self.start_test = True
+                self.x_start = self.position.x
+                self.y_start = self.position.y
+
+    def print_summary(self):
+        rospy.logwarn("~~~~~~Now Start Linear Speed Calibration~~~~~~")
+        rospy.logwarn("-> test_distance: " + str(self.test_distance))
+        rospy.logwarn("-> linear_speed: "  + str(self.speed))
+        rospy.logwarn("-> move_time: "  + str(self.test_distance/self.speed))
+        rospy.logwarn("-> cmd_topic: "  + str(self.cmd_topic))
+        rospy.logwarn("-> distance_tolerance: " + str(self.tolerance))
+        rospy.logwarn("-> linear_scale: " + str(self.odom_linear_scale))
+
+    #get the current transform between the odom and base frames
+    def get_position(self):
+        try:
+            (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
+        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
+            rospy.logerr("lookup TF exception !")
+            return
+        return Point(*trans)
+
+    #Always stop the robot when shutting down the node
+    def shutdown(self):
+        rospy.logwarn("shutdown test node,stopping the robot")
+        self.cmd_vel.publish(Twist())
+        rospy.sleep(1)
+
+if __name__ == '__main__':
+    try:
+        CalibrateLinear()
+        rospy.spin()
+    except:
+        rospy.logerr("Calibration terminated by unknown problems!")
+

rosCode/src/carebot_camera_monitor/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_camera_monitor)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES carebot_camera_monitor
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/carebot_camera_monitor.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/carebot_camera_monitor_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_carebot_camera_monitor.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/carebot_camera_monitor/launch/carebot_camera_monitor.launch

@@ -0,0 +1,4 @@
+<launch>
+  <include file="$(find astra_launch)/launch/astra.launch" />
+  <node name="rviz" type="rviz" pkg="rviz" args="-d $(find carebot_camera_monitor)/rviz/orbbec_camera.rviz" />
+</launch>

rosCode/src/carebot_camera_monitor/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>carebot_camera_monitor</name>
+  <version>0.0.0</version>
+  <description>The carebot_camera_monitor package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/carebot_camera_monitor</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rosCode/src/carebot_camera_monitor/rviz/orbbec_camera.rviz

@@ -0,0 +1,127 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 80
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679016
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Camera
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.0299999993
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /camera/rgb/image_raw
+      Name: Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+      Visibility:
+        Grid: true
+        Value: true
+      Zoom Factor: 1
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: camera_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.0599999987
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.00999999978
+      Pitch: 0.785398006
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.785398006
+    Saved: ~
+Window Geometry:
+  Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 748
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000020b00000262fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000df000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000010d0000017d0000001600ffffff000000010000010f00000262fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000262000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000029f0000026200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 65
+  Y: 24

rosCode/src/carebot_description/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(carebot_description)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

rosCode/src/carebot_description/config/joint_names.yaml

@@ -0,0 +1 @@
+controller_joint_names: []

rosCode/src/carebot_description/config/urdf.rviz

@@ -0,0 +1,172 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.45
+    Tree Height: 359
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_footprint:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_imu_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_footprint:
+          Value: true
+        base_imu_link:
+          Value: true
+        base_link:
+          Value: true
+        laser:
+          Value: true
+        odom_combined:
+          Value: true
+      Marker Scale: 0.3
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom_combined:
+          base_footprint:
+            base_link:
+              base_imu_link:
+                laser:
+                  {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom_combined
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.07899
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.0158278
+        Y: 0.0410318
+        Z: 0.1585
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.115398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 0.122305
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1056
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000019b000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000168fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004100000168000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000007800000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1920
+  X: 0
+  Y: 24

rosCode/src/carebot_description/launch/carebot_description.launch

@@ -0,0 +1,31 @@
+<!--
+  Author: www.corvin.cn
+  Description:配置机器人各link之间关系,在rviz中显示机器人模型.
+  History:
+    20180116:init this launch file.
+ -->
+<launch>
+  <arg name="model" />
+
+  <arg name="gui" default="False" />
+  <param
+    name="robot_description"
+    textfile="$(find carebot_description)/urdf/carebot.urdf" />
+  <param name="use_gui" value="$(arg gui)" />
+
+  <node
+    name="joint_state_publisher"
+    pkg="joint_state_publisher"
+    type="joint_state_publisher">
+    <param name="rate" value="10.0" />
+  </node>
+
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="state_publisher" >
+    <param name="publish_frequency" value="20.0"/>
+  </node>
+
+</launch>
+

rosCode/src/carebot_description/launch/display.launch

@@ -0,0 +1,26 @@
+<launch>
+  <arg name="model" />
+  <arg
+    name="gui"
+    default="False" />
+  <param
+    name="robot_description"
+    textfile="$(find carebot_description)/urdf/carebot.urdf" />
+  <param
+    name="use_gui"
+    value="$(arg gui)" />
+  <node
+    name="joint_state_publisher"
+    pkg="joint_state_publisher"
+    type="joint_state_publisher" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find carebot_description)/config/urdf.rviz" />
+</launch>
+

rosCode/src/carebot_description/package.xml

@@ -0,0 +1,21 @@
+<package>
+  <name>carebot_description</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for carebot_description</p>
+    <p>This package contains configuration data, 3D models and launch files
+for carebot_description robot</p>
+  </description>
+  <author>me</author>
+  <maintainer email="corvin_zhang@corvin.cn" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>gazebo</run_depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>

rosCode/src/carebot_description/urdf/carebot.urdf

@@ -0,0 +1,114 @@
+<robot name="carebot">
+
+  <material name="blue">
+    <color rgba="0.1 0.1 0.75 0.9"/>
+  </material>
+
+  <link name="base_link">
+    <inertial>
+      <origin
+        xyz="-0.00915640899856677 -1.23594917594765E-18 0.116771439156292"
+        rpy="0 0 0" />
+      <mass
+        value="1.53513901604802" />
+      <inertia
+        ixx="0.0429859907839231"
+        ixy="1.40607469244214E-18"
+        ixz="0.00161915133178891"
+        iyy="0.0421680076448672"
+        iyz="-2.37846851589008E-18"
+        izz="0.0250023780420302" />
+    </inertial> 
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://carebot_description/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="package://carebot_description/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="base_footprint">
+    <visual>
+        <geometry>
+          <box size="0.005 0.005 0.005" />
+        </geometry>
+    </visual>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 0.06"/>
+    <parent link="base_footprint"/>
+    <child link="base_link"/>
+  </joint>
+
+  <link name="base_imu_link" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.018 0.003"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="base_link_to_IMU" type="fixed">
+    <origin xyz="0 0 0.094"/>
+    <parent link="base_link"/>
+    <child link="base_imu_link"/>
+  </joint>
+
+  <link name="laser" type="fixed">
+    <inertial>
+      <origin>
+        xyz="0 0 0"
+        rpy="0 0 0"
+      </origin>
+      <mass value="0.19" />
+      <inertia
+        ixx="0.0429859907839231"
+        ixy="1.40607469244214E-18"
+        ixz="0.00161915133178891"
+        iyy="0.0421680076448672"
+        iyz="-2.37846851589008E-18"
+        izz="0.0250023780420302" />
+    </inertial>
+    <visual>
+      <origin>
+        xyz="0 0 0"
+        rpy="1.57079 0 0"
+      </origin>
+      <geometry>
+        <cylinder length="0.042" radius="0.038" />
+      </geometry>
+      <material name="">
+        <color rgba="0.79216 0.81961 0.9333 0.5" />
+      </material>
+    </visual>
+    <collision>
+      <origin>
+        xyz="0 0 0"
+        rpy="1.57.79 0 0"
+      </origin>
+      <geometry>
+        <cylinder length="0.042" radius="0.038" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="lidar_to_imu" type="fixed">
+    <origin xyz="0 0 0.155"/>
+    <parent link="base_imu_link"/>
+    <child link="laser"/>
+  </joint>
+
+</robot>

rosCode/src/carebot_description/urdf/carebot.xacro

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<!--
+  Author: corvin
+  Description: this file is about carebot description
+  Date: 20171222-init this description files;
+-->
+
+<robot name="carebot" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find carebot_description)/urdf/common_properties.urdf.xacro" />
+  <xacro:property name="M_SCALE" value="0.001" />
+
+  <xacro:include filename="$(find carebot_description)/urdf/carebot_stack.xacro" />
+  <!--<xacro include filename="$(find carebot_description)/urdf/rplidar_a2.urdf" />-->
+
+</robot>

rosCode/src/carebot_description/urdf/carebot_stack.xacro

@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<!--
+  Author: corvin
+  Description: this is carebot stack urdf file
+  Date: 20171222-init file;
+-->
+
+<robot name="carebot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <link name="base_footprint"/>
+    <!--
+       Base link is set at the bottom of the base mould.
+       This is done to be compatible with the way base link
+       was configured for turtlebot 1. Refer to
+
+       https://github.com/turtlebot/turtlebot/issues/40
+
+       To put the base link at the more oft used wheel
+       axis, set the z-distance from the base_footprint
+       to 0.352.
+      -->
+    <joint name="base_joint" type="fixed">
+      <origin xyz="0 0 0.0102" rpy="0 0 0" />
+      <parent link="base_footprint"/>
+      <child link="base_link" />
+    </joint>
+    <link name="base_link">
+      <visual>
+        <geometry>
+          <!-- new mesh -->
+          <mesh filename="package://carebot_description/meshes/base_link.STL" />
+        </geometry>
+        <origin xyz="0.001 0 0.05199" rpy="0 0 0"/>
+      </visual>
+      <collision name="base">
+        <geometry>
+          <cylinder length="0.10938" radius="0.178"/>
+        </geometry>
+        <origin xyz="0.0 0 0.05949" rpy="0 0 0"/>
+      </collision>
+      <inertial>
+        <!-- COM experimentally determined -->
+        <origin xyz="0.01 0 0"/>
+        <mass value="2.4"/> <!-- 2.4/2.6 kg for small/big battery pack -->
+        <inertia ixx="0.019995" ixy="0.0" ixz="0.0"
+                 iyy="0.019995" iyz="0.0"
+                 izz="0.03675" />
+      </inertial>
+    </link>
+
+</robot>

rosCode/src/carebot_description/urdf/common_properties.urdf.xacro

@@ -0,0 +1,57 @@
+<?xml version="1.0" ?>
+<!--
+  Various useful properties such as constants and materials 
+ -->
+<robot name="xacro_properties" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:property name="M_PI" value="3.1415926535897931" /> 
+  <xacro:property name="material_white">
+    <material name="white">
+      <color rgba="1 1 1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_grey">
+    <material name="grey">
+      <color rgba="0.5 0.5 0.5 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_light_grey">
+    <material name="light_grey">
+      <color rgba="0.6 0.6 0.6 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_dark_grey">
+    <material name="dark_grey">
+      <color rgba="0.3 0.3 0.3 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_black">
+    <material name="black">
+      <color rgba="0.1 0.1 0.1 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_red">
+    <material name="red">
+      <color rgba="1 0 0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_green">
+    <material name="green">
+      <color rgba="0.0 0.8 0.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_blue">
+    <material name="blue">
+      <color rgba="0.0 0.0 1.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_grey_blue">
+    <material name="grey_blue">
+      <color rgba="0.4 0.4 1.0 1"/>
+    </material>
+  </xacro:property>
+  <xacro:property name="material_orange">
+    <material name="orange">
+      <color rgba="1.0 0.7 0.0 1"/>
+    </material>
+  </xacro:property>
+</robot>

rosCode/src/carebot_follower/.gitignore

@@ -0,0 +1,3 @@
+/bin
+/docs
+/lib

rosCode/src/carebot_follower/CHANGELOG.rst

@@ -0,0 +1,86 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package turtlebot_follower
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.3.7 (2016-11-01)
+------------------
+* fixed bad printf format
+* clean up cmake dependencies
+* Included depth_image_proc in cmake deps
+* Contributors: Mr-Yellow, Rohan Agrawal, Tully Foote
+
+2.3.6 (2016-06-29)
+------------------
+* add dependency on depth_image_proc
+* Contributors: Tully Foote
+
+2.3.5 (2016-06-28)
+------------------
+
+2.3.4 (2016-06-28)
+------------------
+* Switch to use depth/image_rect instead of depth/points for efficiency.
+  It takes ~ 1/4th the cpu power to do this version.
+  This has the added benefit of not requiring PCL as a dependency.
+* Add support for joystick override of follower
+  This allows joystick button 5 to toggle follower on and off.
+  It simply adds another input to the mux which sends zero
+  velocity with a slightly higher priority than the follower.
+* the sim directory no longer exists
+  It was implicitly removed in `#142 <https://github.com/turtlebot/turtlebot_apps/issues/142>`_
+* Added arguments in launch file
+* Add simulation launch in turtlebot_follower
+* Contributors: Jihoon Lee, Nadya Ampilogova, Tully Foote
+
+2.3.3 (2015-03-23)
+------------------
+
+2.3.2 (2015-01-21)
+------------------
+
+2.3.1 (2014-12-30)
+------------------
+
+2.3.0 (2014-12-30)
+------------------
+* remove inappropriate / fixes `#127 <https://github.com/turtlebot/turtlebot_apps/issues/127>`_
+* sim to simulation param
+* follower in simulation
+* Contributors: Jihoon Lee
+
+2.2.4 (2013-10-14)
+------------------
+
+2.2.3 (2013-09-27)
+------------------
+
+2.2.2 (2013-09-26)
+------------------
+* Use service files to turtlebot_msgs.
+
+2.2.1 (2013-09-23)
+------------------
+
+2.2.0 (2013-08-30)
+------------------
+* Rename include launchers to *.launch.xml.
+* Changelogs at package level.
+
+2.1.x - hydro, unstable
+=======================
+
+2.1.1 (2013-08-09)
+------------------
+* Fix namespace issues
+* Rationalize the use of velocity smoother: remap properly robot_cmd_vel, add comments, and avoid meaningless topic names
+
+2.1.0 (2013-07-19)
+------------------
+* Catkinized
+* Unexistent include dir removed from package description
+
+
+Previous versions, bugfixing
+============================
+
+Available in ROS wiki: http://ros.org/wiki/turtlebot_apps/ChangeList

rosCode/src/carebot_follower/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_follower)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS nodelet roscpp visualization_msgs carebot_msgs depth_image_proc dynamic_reconfigure)
+find_package(Boost REQUIRED)
+
+generate_dynamic_reconfigure_options(cfg/Follower.cfg)
+
+catkin_package(
+  INCLUDE_DIRS
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS nodelet roscpp visualization_msgs carebot_msgs depth_image_proc dynamic_reconfigure
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(${PROJECT_NAME} src/follower.cpp)
+
+add_dependencies(${PROJECT_NAME}
+  ${catkin_EXPORTED_TARGETS}
+  ${PROJECT_NAME}_gencfg
+  ${carebot_msgs_EXPORTED_TARGETS}
+)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+## Mark executables and/or libraries for installation
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark all other required files for installation
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY param
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY plugins
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(PROGRAMS
+  scripts/switch.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_carebot_follower.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/carebot_follower/cfg/.gitignore

@@ -0,0 +1 @@
+/cpp

rosCode/src/carebot_follower/cfg/Follower.cfg

@@ -0,0 +1,52 @@
+#! /usr/bin/env python                                                                                          
+# Software License Agreement (BSD License)                                                                      
+#                                                                                                               
+# Copyright (c) 2009, Willow Garage, Inc.                                                                       
+# All rights reserved.                                                                                          
+#                                                                                                               
+# Redistribution and use in source and binary forms, with or without                                            
+# modification, are permitted provided that the following conditions                                            
+# are met:                                                                                                      
+#                                                                                                               
+#  * Redistributions of source code must retain the above copyright                                             
+#    notice, this list of conditions and the following disclaimer.                                              
+#  * Redistributions in binary form must reproduce the above                                                    
+#    copyright notice, this list of conditions and the following                                                
+#    disclaimer in the documentation and/or other materials provided                                            
+#    with the distribution.                                                                                     
+#  * Neither the name of Willow Garage, Inc. nor the names of its                                               
+#    contributors may be used to endorse or promote products derived                                            
+#    from this software without specific prior written permission.                                              
+#                                                                                                               
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS                                           
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT                                             
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS                                             
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE                                                
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                                           
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,                                          
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                                              
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER                                              
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT                                            
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN                                             
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE                                               
+# POSSIBILITY OF SUCH DAMAGE.                                                                                   
+
+
+PACKAGE='carebot_follower'
+
+import math
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+gen.add("min_x", double_t, 0, "The minimum x position of the points in the box.", -0.20, -3.0,  3.0)
+gen.add("max_x", double_t, 0, "The maximum x position of the points in the box.",  0.20, -3.0, 3.0)
+gen.add("min_y", double_t, 0, "The minimum y position of the points in the box.", 0.10, -1.0, 3.0)
+gen.add("max_y", double_t, 0, "The maximum y position of the points in the box.", 0.50, -1.0, 3.0)
+gen.add("max_z", double_t, 0, "The maximum z position of the points in the box.", 0.8, 0.0, 3.0)
+gen.add("goal_z", double_t, 0, "The distance away from the robot to hold the centroid.", 0.6, 0.0, 3.0)
+gen.add("x_scale", double_t, 0, "The scaling factor for translational robot speed.", 1.0, 0.0, 3.0)
+gen.add("z_scale", double_t, 0, "The scaling factor for rotational robot speed.", 5.0, 0.0, 10.0)
+
+
+exit(gen.generate(PACKAGE, "carebot_follower_dynamic_reconfigure", "Follower"))

rosCode/src/carebot_follower/launch/follower.launch

@@ -0,0 +1,74 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+    The carebot people (or whatever) follower nodelet with orrbec rgbd sensor.
+    first roslaunch carebot_bringup.launch, then startup follower function. 
+  History:
+      20180411:init this file.
+-->
+
+<launch>
+  <!-- first roslaunch carebot_bringup.launch -->
+  <include file="$(find carebot_bringup)/launch/carebot_bringup.launch" />
+
+  <arg name="simulation" default="false"/>
+  <group unless="$(arg simulation)"> <!-- Real robot -->
+    <include file="$(find carebot_follower)/launch/includes/velocity_smoother.launch.xml">
+      <arg name="nodelet_manager"  value="/mobile_base_nodelet_manager"/>
+      <arg name="navigation_topic" value="/cmd_vel_mux/input/navi"/>
+    </include>
+
+    <include file="$(find astra_launch)/launch/astra.launch">
+      <arg name="rgb_processing"                  value="true"/>  <!-- only required if we use android client -->
+      <arg name="depth_processing"                value="true"/>
+      <arg name="depth_registered_processing"     value="false"/>
+      <arg name="depth_registration"              value="false"/>
+      <arg name="disparity_processing"            value="false"/>
+      <arg name="disparity_registered_processing" value="false"/>
+    </include>
+  </group>
+
+  <group if="$(arg simulation)">
+    <!-- Load nodelet manager for compatibility -->
+    <node pkg="nodelet" type="nodelet" ns="camera" name="camera_nodelet_manager" args="manager"/>
+
+    <include file="$(find carebot_follower)/launch/includes/velocity_smoother.launch.xml">
+      <arg name="nodelet_manager"  value="camera/camera_nodelet_manager"/>
+      <arg name="navigation_topic" value="cmd_vel_mux/input/navi"/>
+    </include>
+  </group>
+
+  <param name="camera/rgb/image_color/compressed/jpeg_quality" value="22"/>
+
+  <!-- Make a slower camera feed available; only required if we use android client -->
+  <node pkg="topic_tools" type="throttle" name="camera_throttle"
+        args="messages camera/rgb/image_color/compressed 5"/>
+
+  <include file="$(find carebot_follower)/launch/includes/safety_controller.launch.xml"/>
+
+  <!--  Real robot: Load carebot follower into the 3d sensors nodelet manager to avoid pointcloud serializing -->
+  <!--  Simulation: Load carebot follower into nodelet manager for compatibility -->
+  <node pkg="nodelet" type="nodelet" name="carebot_follower"
+        args="load carebot_follower/TurtlebotFollower camera/camera_nodelet_manager">
+
+    <!--<remap from="carebot_follower/cmd_vel" to="follower_velocity_smoother/raw_cmd_vel"/>-->
+    <remap from="carebot_follower/cmd_vel" to="cmd_vel" />
+
+    <remap from="depth/points" to="camera/depth/points"/>
+    <param name="enabled" value="true" />
+    <param name="x_scale" value="7.0" />
+    <param name="z_scale" value="2.0" />
+    <param name="min_x" value="-0.35" />
+    <param name="max_x" value="0.35" />
+    <param name="min_y" value="0.1" />
+    <param name="max_y" value="0.5" />
+    <param name="max_z" value="1.2" />
+    <param name="goal_z" value="0.6" />
+  </node>
+
+  <!-- Launch the script which will toggle carebot following on and off based on a joystick button. default: on -->
+  <node name="switch" pkg="carebot_follower" type="switch.py"/>
+
+</launch>
+

rosCode/src/carebot_follower/launch/includes/safety_controller.launch.xml

@@ -0,0 +1,11 @@
+<launch>
+  <!-- safety controller -->
+  <!-- Disabling for now - in practice it doesn't seem to be working as conveniently as expected.
+  <node pkg="nodelet" type="nodelet" name="kobuki_safety_controller" args="load kobuki_safety_controller/SafetyControllerNodelet mobile_base_nodelet_manager">
+    <remap from="kobuki_safety_controller/cmd_vel" to="cmd_vel_mux/input/safety_controller"/>
+    <remap from="kobuki_safety_controller/events/bumper" to="mobile_base/events/bumper"/>
+    <remap from="kobuki_safety_controller/events/cliff" to="mobile_base/events/cliff"/>
+    <remap from="kobuki_safety_controller/events/wheel_drop" to="mobile_base/events/wheel_drop"/>
+  </node>
+  -->
+</launch>

rosCode/src/carebot_follower/launch/includes/velocity_smoother.launch.xml

@@ -0,0 +1,25 @@
+<!--
+         Velocity smoother
+-->
+<launch>
+  <arg name="nodelet_manager"/>
+  <arg name="navigation_topic"/>
+
+  <node pkg="nodelet" type="nodelet" name="follower_velocity_smoother"
+        args="load yocs_velocity_smoother/VelocitySmootherNodelet $(arg nodelet_manager)">
+    <rosparam file="$(find carebot_follower)/param/smoother.yaml" command="load"/>
+    <remap from="follower_velocity_smoother/smooth_cmd_vel" to="$(arg navigation_topic)"/>
+
+    <!-- Robot velocity feedbacks; use the default base configuration -->
+    <remap from="follower_velocity_smoother/odometry" to="/odom"/>
+    <remap from="follower_velocity_smoother/robot_cmd_vel" to="/mobile_base/commands/velocity"/>
+
+    <!-- connecting between follower/cmd_vel in mobile_base_nodelet and smoother/raw_cmd_vel in camera_nodelet -->
+<!--     <remap from="follower_velocity_smoother/raw_cmd_vel" to="/mobile_base_nodelet_camera_nodelet/raw_cmd_vel"/> -->
+
+    <!-- Configure velocity smoother to allow more aggressive motion -->
+    <param name="accel_lim_v"  value="0.1"/>
+    <param name="accel_lim_w"  value="0.5"/>
+    <param name="decel_factor" value="0.5"/>
+  </node>
+</launch>

rosCode/src/carebot_follower/package.xml

@@ -0,0 +1,34 @@
+<package>
+  <name>carebot_follower</name>
+  <version>2.3.7</version>
+  <description>
+       Follower for the carebot. Follows humans and robots around by following the centroid of a box points in front of the carebot.
+  </description>
+
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
+
+  <license>BSD</license>
+
+  <author email="corvin_zhang@corvin.cn">corvin zhang</author> 
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>depth_image_proc</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>carebot_msgs</build_depend>
+  
+  <run_depend>depth_image_proc</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>topic_tools</run_depend>
+  <run_depend>carebot_msgs</run_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/plugins/nodelets.xml" />
+  </export>
+</package>

rosCode/src/carebot_follower/param/mux.yaml

@@ -0,0 +1,20 @@
+# Created on: Dec 5, 2012
+#     Author: jorge
+# Configuration for subscribers to cmd_vel sources.
+#
+# Individual subscriber configuration:
+#   name:           Source name
+#   topic:          The topic that provides cmd_vel messages
+#   timeout:        Time in seconds without incoming messages to consider this topic inactive
+#   priority:       Priority: an UNIQUE unsigned integer from 0 (lowest) to MAX_INT 
+#   short_desc:     Short description (optional)
+
+subscribers:
+  - name:        "Safe reactive controller"
+    topic:       "input/safety_controller"
+    timeout:     0.2
+    priority:    10
+  - name:        "Follower"
+    topic:       "input/follower"
+    timeout:     0.5
+    priority:    7

rosCode/src/carebot_follower/param/smoother.yaml

@@ -0,0 +1,20 @@
+# Default parameters used by the yocs_velocity_smoother module.
+# This isn't used by minimal.launch per se, rather by everything
+# which runs on top.
+
+# Mandatory parameters
+speed_lim_v: 0.17
+speed_lim_w: 1.0
+
+accel_lim_v: 0.1 # maximum is actually 2.0, but we push it down to be smooth
+accel_lim_w: 0.2
+
+# Optional parameters
+frequency: 10.0
+decel_factor: 0.5
+
+# Robot velocity feedback type:
+#  0 - none (default)
+#  1 - odometry
+#  2 - end robot commands
+robot_feedback: 2

rosCode/src/carebot_follower/plugins/nodelets.xml

@@ -0,0 +1,7 @@
+<library path="lib/libcarebot_follower">
+  <class name="carebot_follower/TurtlebotFollower" type="carebot_follower::TurtlebotFollower" base_class_type="nodelet::Nodelet">
+    <description>
+      The carebot people follower node.
+    </description>
+  </class>
+</library> 

rosCode/src/carebot_follower/scripts/switch.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+# license removed for brevity
+import rospy
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Joy
+
+# This script will listen for joystick button 5 being toggled and
+# send zero speed messages to the mux to disable the follower until
+# button 5 is pressed again.
+
+class BehaviorSwitch(object):
+    def __init__(self):
+        self.running = False
+
+    def callback(self, joy_msg):
+        if joy_msg.buttons[5] == 1:
+            self.running = not self.running
+
+        rospy.loginfo(repr(joy_msg))
+
+    def run(self):
+        rospy.init_node('behavior_switch', anonymous=True)
+        pub = rospy.Publisher('cmd_vel_mux/input/switch', Twist, queue_size=10)
+        rospy.Subscriber('joy', Joy, self.callback)
+        rate = rospy.Rate(10)
+        while not rospy.is_shutdown():
+            if self.running:
+                empty_msg = Twist()
+                pub.publish(empty_msg)
+            rate.sleep()
+
+if __name__ == '__main__':
+    try:
+        behavior_switch = BehaviorSwitch()
+        behavior_switch.run()
+    except rospy.ROSInterruptException:
+        pass

rosCode/src/carebot_follower/src/.gitignore

@@ -0,0 +1 @@
+/turtlebot_follower

rosCode/src/carebot_follower/src/follower.cpp

@@ -0,0 +1,319 @@
+/*
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <ros/ros.h>
+#include <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/Image.h>
+#include <visualization_msgs/Marker.h>
+#include <carebot_msgs/SetFollowState.h>
+
+#include "dynamic_reconfigure/server.h"
+#include "carebot_follower/FollowerConfig.h"
+
+#include <depth_image_proc/depth_traits.h>
+
+
+namespace carebot_follower
+{
+
+//* The carebot follower nodelet.
+/**
+ * The carebot follower nodelet. Subscribes to point clouds
+ * from the 3dsensor, processes them, and publishes command vel
+ * messages.
+ */
+class TurtlebotFollower : public nodelet::Nodelet
+{
+public:
+  /*!
+   * @brief The constructor for the follower.
+   * Constructor for the follower.
+   */
+  TurtlebotFollower() : min_y_(0.1), max_y_(0.5),
+                        min_x_(-0.2), max_x_(0.2),
+                        max_z_(0.8), goal_z_(0.6),
+                        z_scale_(1.0), x_scale_(5.0)
+  {
+
+  }
+
+  ~TurtlebotFollower()
+  {
+    delete config_srv_;
+  }
+
+private:
+  double min_y_; /**< The minimum y position of the points in the box. */
+  double max_y_; /**< The maximum y position of the points in the box. */
+  double min_x_; /**< The minimum x position of the points in the box. */
+  double max_x_; /**< The maximum x position of the points in the box. */
+  double max_z_; /**< The maximum z position of the points in the box. */
+  double goal_z_; /**< The distance away from the robot to hold the centroid */
+  double z_scale_; /**< The scaling factor for translational robot speed */
+  double x_scale_; /**< The scaling factor for rotational robot speed */
+  bool   enabled_; /**< Enable/disable following; just prevents motor commands */
+
+  // Service for start/stop following
+  ros::ServiceServer switch_srv_;
+
+  // Dynamic reconfigure server
+  dynamic_reconfigure::Server<carebot_follower::FollowerConfig>* config_srv_;
+
+  /*!
+   * @brief OnInit method from node handle.
+   * OnInit method from node handle. Sets up the parameters
+   * and topics.
+   */
+  virtual void onInit()
+  {
+    ros::NodeHandle& nh = getNodeHandle();
+    ros::NodeHandle& private_nh = getPrivateNodeHandle();
+
+    private_nh.getParam("min_y", min_y_);
+    private_nh.getParam("max_y", max_y_);
+    private_nh.getParam("min_x", min_x_);
+    private_nh.getParam("max_x", max_x_);
+    private_nh.getParam("max_z", max_z_);
+    private_nh.getParam("goal_z", goal_z_);
+    private_nh.getParam("z_scale", z_scale_);
+    private_nh.getParam("x_scale", x_scale_);
+    private_nh.getParam("enabled", enabled_);
+
+    cmdpub_ = private_nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
+    markerpub_ = private_nh.advertise<visualization_msgs::Marker>("marker",1);
+    bboxpub_ = private_nh.advertise<visualization_msgs::Marker>("bbox",1);
+    sub_= nh.subscribe<sensor_msgs::Image>("depth/image_rect", 1, &TurtlebotFollower::imagecb, this);
+
+    switch_srv_ = private_nh.advertiseService("change_state", &TurtlebotFollower::changeModeSrvCb, this);
+
+    config_srv_ = new dynamic_reconfigure::Server<carebot_follower::FollowerConfig>(private_nh);
+    dynamic_reconfigure::Server<carebot_follower::FollowerConfig>::CallbackType f =
+        boost::bind(&TurtlebotFollower::reconfigure, this, _1, _2);
+    config_srv_->setCallback(f);
+  }
+
+  void reconfigure(carebot_follower::FollowerConfig &config, uint32_t level)
+  {
+    min_y_ = config.min_y;
+    max_y_ = config.max_y;
+    min_x_ = config.min_x;
+    max_x_ = config.max_x;
+    max_z_ = config.max_z;
+    goal_z_ = config.goal_z;
+    z_scale_ = config.z_scale;
+    x_scale_ = config.x_scale;
+  }
+
+  /*!
+   * @brief Callback for point clouds.
+   * Callback for depth images. It finds the centroid
+   * of the points in a box in the center of the image. 
+   * Publishes cmd_vel messages with the goal from the image.
+   * @param cloud The point cloud message.
+   */
+  void imagecb(const sensor_msgs::ImageConstPtr& depth_msg)
+  {
+
+    // Precompute the sin function for each row and column
+    uint32_t image_width = depth_msg->width;
+    float x_radians_per_pixel = 60.0/57.0/image_width;
+    float sin_pixel_x[image_width];
+    for (int x = 0; x < image_width; ++x) {
+      sin_pixel_x[x] = sin((x - image_width/ 2.0)  * x_radians_per_pixel);
+    }
+
+    uint32_t image_height = depth_msg->height;
+    float y_radians_per_pixel = 45.0/57.0/image_width;
+    float sin_pixel_y[image_height];
+    for (int y = 0; y < image_height; ++y) {
+      // Sign opposite x for y up values
+      sin_pixel_y[y] = sin((image_height/ 2.0 - y)  * y_radians_per_pixel);
+    }
+
+    //X,Y,Z of the centroid
+    float x = 0.0;
+    float y = 0.0;
+    float z = 1e6;
+    //Number of points observed
+    unsigned int n = 0;
+
+    //Iterate through all the points in the region and find the average of the position
+    const float* depth_row = reinterpret_cast<const float*>(&depth_msg->data[0]);
+    int row_step = depth_msg->step / sizeof(float);
+    for (int v = 0; v < (int)depth_msg->height; ++v, depth_row += row_step)
+    {
+     for (int u = 0; u < (int)depth_msg->width; ++u)
+     {
+       float depth = depth_image_proc::DepthTraits<float>::toMeters(depth_row[u]);
+       if (!depth_image_proc::DepthTraits<float>::valid(depth) || depth > max_z_) continue;
+       float y_val = sin_pixel_y[v] * depth;
+       float x_val = sin_pixel_x[u] * depth;
+       if ( y_val > min_y_ && y_val < max_y_ &&
+            x_val > min_x_ && x_val < max_x_)
+       {
+         x += x_val;
+         y += y_val;
+         z = std::min(z, depth); //approximate depth as forward.
+         n++;
+       }
+     }
+    }
+
+    //If there are points, find the centroid and calculate the command goal.
+    //If there are no points, simply publish a stop goal.
+    if (n>4000)
+    {
+      x /= n;
+      y /= n;
+      if(z > max_z_){
+        ROS_INFO_THROTTLE(1, "Centroid too far away %f, stopping the robot", z);
+        if (enabled_)
+        {
+          cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
+        }
+        return;
+      }
+
+      ROS_INFO_THROTTLE(1, "Centroid at %f %f %f with %d points", x, y, z, n);
+      publishMarker(x, y, z);
+
+      if (enabled_)
+      {
+        geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
+        cmd->linear.x = (z - goal_z_) * z_scale_;
+        cmd->angular.z = -x * x_scale_;
+        cmdpub_.publish(cmd);
+      }
+    }
+    else
+    {
+      ROS_INFO_THROTTLE(1, "Not enough points(%d) detected, stopping the robot", n);
+      publishMarker(x, y, z);
+
+      if (enabled_)
+      {
+        cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
+      }
+    }
+
+    publishBbox();
+  }
+
+  bool changeModeSrvCb(carebot_msgs::SetFollowState::Request& request,
+                       carebot_msgs::SetFollowState::Response& response)
+  {
+    if ((enabled_ == true) && (request.state == request.STOPPED))
+    {
+      ROS_INFO("Change mode service request: following stopped");
+      cmdpub_.publish(geometry_msgs::TwistPtr(new geometry_msgs::Twist()));
+      enabled_ = false;
+    }
+    else if ((enabled_ == false) && (request.state == request.FOLLOW))
+    {
+      ROS_INFO("Change mode service request: following (re)started");
+      enabled_ = true;
+    }
+
+    response.result = response.OK;
+    return true;
+  }
+
+  void publishMarker(double x,double y,double z)
+  {
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "/camera_rgb_optical_frame";
+    marker.header.stamp = ros::Time();
+    marker.ns = "my_namespace";
+    marker.id = 0;
+    marker.type = visualization_msgs::Marker::SPHERE;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.pose.position.x = x;
+    marker.pose.position.y = y;
+    marker.pose.position.z = z;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    marker.scale.x = 0.2;
+    marker.scale.y = 0.2;
+    marker.scale.z = 0.2;
+    marker.color.a = 1.0;
+    marker.color.r = 1.0;
+    marker.color.g = 0.0;
+    marker.color.b = 0.0;
+    //only if using a MESH_RESOURCE marker type:
+    markerpub_.publish( marker );
+  }
+
+  void publishBbox()
+  {
+    double x = (min_x_ + max_x_)/2;
+    double y = (min_y_ + max_y_)/2;
+    double z = (0 + max_z_)/2;
+
+    double scale_x = (max_x_ - x)*2;
+    double scale_y = (max_y_ - y)*2;
+    double scale_z = (max_z_ - z)*2;
+
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "/camera_rgb_optical_frame";
+    marker.header.stamp = ros::Time();
+    marker.ns = "my_namespace";
+    marker.id = 1;
+    marker.type = visualization_msgs::Marker::CUBE;
+    marker.action = visualization_msgs::Marker::ADD;
+    marker.pose.position.x = x;
+    marker.pose.position.y = -y;
+    marker.pose.position.z = z;
+    marker.pose.orientation.x = 0.0;
+    marker.pose.orientation.y = 0.0;
+    marker.pose.orientation.z = 0.0;
+    marker.pose.orientation.w = 1.0;
+    marker.scale.x = scale_x;
+    marker.scale.y = scale_y;
+    marker.scale.z = scale_z;
+    marker.color.a = 0.5;
+    marker.color.r = 0.0;
+    marker.color.g = 1.0;
+    marker.color.b = 0.0;
+    //only if using a MESH_RESOURCE marker type:
+    bboxpub_.publish( marker );
+  }
+
+  ros::Subscriber sub_;
+  ros::Publisher cmdpub_;
+  ros::Publisher markerpub_;
+  ros::Publisher bboxpub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(carebot_follower::TurtlebotFollower, nodelet::Nodelet)
+
+}

rosCode/src/carebot_msgs/CHANGELOG.rst

@@ -0,0 +1,21 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package turtlebot_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+2.2.1 (2015-04-06)
+------------------
+* remove laptop charge status message `#2 <https://github.com/turtlebot/turtlebot_msgs/issues/2>`_
+* LaptopChargeStatus moved here from turtlebot stack.
+* Contributors: Daniel Stonier, Jihoon Lee
+
+2.2.0 (2013-08-30)
+------------------
+* Changelog added
+
+
+2.1.x - hydro, unstable
+=======================
+
+2.1.0 (2013-08-16)
+------------------
+* First release.

rosCode/src/carebot_msgs/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs std_srvs message_generation)
+
+add_message_files(DIRECTORY msg
+                      FILES PanoramaImg.msg
+                 )
+
+add_service_files(DIRECTORY srv
+                     FILES TakePanorama.srv
+                           SetFollowState.srv
+                 )
+
+generate_messages(DEPENDENCIES std_msgs sensor_msgs)
+
+catkin_package(CATKIN_DEPENDS std_msgs sensor_msgs std_srvs message_runtime)

rosCode/src/carebot_msgs/README.md

@@ -0,0 +1,4 @@
+carebot_msgs
+==============
+
+carebot messages, services and actions

rosCode/src/carebot_msgs/msg/PanoramaImg.msg

@@ -0,0 +1,8 @@
+#Pano message
+Header header
+string pano_id
+float64 latitude
+float64 longitude
+float64 heading #in degrees, compass heading
+string geo_tag
+sensor_msgs/Image image

rosCode/src/carebot_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+  <name>carebot_msgs</name>
+  <version>2.2.1</version>
+  <description>carebot messages, services and actions</description>
+
+  <author email="corvin_zhang@corvin.cn">corvin zhang</author>
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
+  <license>BSD</license>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>std_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+</package>

rosCode/src/carebot_msgs/srv/SetFollowState.srv

@@ -0,0 +1,12 @@
+uint8 STOPPED = 0
+uint8 FOLLOW  = 1
+
+# Following running/stopped
+uint8 state
+
+---
+
+uint8 OK    = 0
+uint8 ERROR = 1
+
+uint8 result

rosCode/src/carebot_msgs/srv/TakePanorama.srv

@@ -0,0 +1,21 @@
+# mode for taking the pictures
+uint8 mode
+# rotate, stop, snapshot, rotate, stop, snapshot, ...
+uint8 SNAPANDROTATE=0
+# keep rotating while taking snapshots
+uint8 CONTINUOUS=1
+# stop an ongoing panorama creation
+uint8 STOP=2
+# total angle of panorama picture
+float32 pano_angle
+# angle interval when creating the panorama picture in snap&rotate mode, time interval otherwise 
+float32 snap_interval
+# rotating velocity
+float32 rot_vel
+
+---
+
+uint8 status
+uint8 STARTED=0
+uint8 IN_PROGRESS=1
+uint8 STOPPED=2

rosCode/src/carebot_navigation/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(carebot_navigation)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES navigation
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/navigation.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/navigation_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_navigation.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

rosCode/src/carebot_navigation/config/base_local_planner_params.yaml

@@ -0,0 +1,69 @@
+# Copyright(c): 2016-2018 ROS小课堂
+# Author: www.corvin.cn
+# Description:
+#  本地规划配置文件,各参数意义如下：
+#   controller_frequency:计算得到路径以多少频率向cmd_vel发布.
+#   max_vel_x:最大前进速度.
+#   min_vel_x:最小前进速度.
+#   max_vel_y:最大横向移动速度.
+#   min_vel_y:最小横向移动速度.
+#   max_vel_theta:最大旋转角速度.
+#   min_in_place_vel_theta:机器人最小原地旋转速度.
+#   escape_vel: 机器人后退速度,该值必须是负数. 
+# History:
+#   20180410: init this file.
+#   20180411: update param based on kinds.
+#
+
+recovery_behavior_enabled: true
+clearing_rotation_allowed: false
+
+TrajectoryPlannerROS:
+   #Robot Configuration Parameters
+   max_vel_x: 0.17
+   min_vel_x: 0.12
+   y_vels: [-0.17,-0.1,0.1,0.17]
+   max_vel_theta: 0.5
+   min_vel_theta: -0.5
+   min_in_place_vel_theta: 0.3
+   escape_vel: -0.12
+
+   acc_lim_x: 0.05
+   acc_lim_y: 0.05   # accelerator speed 
+   acc_lim_theta: 0.1
+
+   holonomic_robot: true
+
+   #Goal Tolerance Parameters
+   xy_goal_tolerance: 0.30  # 30cm
+   yaw_goal_tolerance: 0.3  # about 17 degrees
+   latch_xy_goal_tolerance: true
+
+   #Forward Simulation Parameters
+   sim_time: 2.0
+   sim_granularity: 0.025
+   angular_sim_granularity: 0.025
+   vx_samples: 5
+   vy_samples: 3 # omniwheel robot
+   vtheta_samples: 12 
+   controller_frequency: 4.0
+   
+   #Trajectory Scoring Parameters
+   meter_scoring: true
+   pdist_scale: 0.8
+   gdist_scale: 0.5
+   occdist_scale: 0.1
+
+   heading_lookahead: 0.325
+   heading_scoring: false
+   heading_scoring_timestep: 0.8
+   dwa: true
+   publish_cost_grid_pc: false
+
+   #Oscillation Prevention Parameters
+   oscillation_timeout: 3.0
+   oscillation_reset_dist: 0.5
+
+   #Global Plan Parameters
+   prune_plan: true
+   

rosCode/src/carebot_navigation/config/costmap_common_params.yaml

@@ -0,0 +1,20 @@
+#Copyright(c):2016-2018 ROS小课堂
+#Author: www.corvin.cn
+#Description:
+# 代价地图通用配置文件，各参数意义如下：
+#   obstacle_range: 最大障碍物检测范围,超过该范围不认为是障碍物
+#   raytrace_range: 检测自由空间的最大范围
+#   robot_radius: 机器人本体的半径
+#   inflation_radius: 与障碍物的安全半径距离,如果机器人经常撞到
+#     障碍物就需要增大该值,若无法通过狭窄地方就减小该值
+#   observation_sources: 观察源是激光雷达
+# History:
+#  20180410: init this file.
+
+obstacle_range: 2.0
+raytrace_range: 3.0
+robot_radius: 0.18
+inflation_radius: 0.2
+observation_sources: scan
+scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
+

rosCode/src/carebot_navigation/config/global_costmap_params.yaml

@@ -0,0 +1,24 @@
+#Copyright(C): 2016-2018 ROS小课堂
+#Author: www.corvin.cn
+#Description:
+#  全局代价地图配置文件，各参数意义如下：
+#  global_frame: 全局代价地图参考系
+#  robot_base_frame: 指定机器人的基础参考系
+#  update_frequency: 指定地图更新频率，数值太大造成CPU负担重
+#  publish_frequency: 对于静态全局地图来说，不需不断发布
+#  static_map: 全局地图通常是静态的，因此设置为true
+#  rolling_window: 全局地图不会在机器人移动时候更新
+#  transform_tolerance: 指定在tf树中frame直接的转换最大延时，单位秒
+#History:
+#  20180410: init this file.
+#
+
+global_costmap:
+   global_frame: /map
+   robot_base_frame: /base_footprint
+   update_frequency: 2.0
+   publish_frequency: 0
+   static_map: true
+   rolling_window: false
+   transform_tolerance: 2.0
+

rosCode/src/carebot_navigation/config/local_costmap_params.yaml

@@ -0,0 +1,30 @@
+#Copyright(c):2016-2018 ROS小课堂
+#Author: www.corvin.cn
+#Description:
+#  本地代价地图配置文件，各参数意义如下：
+#  global_frame:指定本地代价地图参考系为odom
+#  robot_base_frame:指定机器人的base_frame
+#  update_frequency:指定地图更新频率
+#  publish_frequency: 代价地图发布可视化信息的频率
+#  static_map: 本地代价地图不会更新地图，设置false
+#  rolling_window: 设置滚动窗口,使机器人始终在窗体中心位置
+#  width: 代价地图宽度,滑动地图x维长度
+#  height:代价地图长度,滑动地图y维长度
+#  resolution: 代价地图的分辨率,该参数需要与yaml文件设置的地图
+#    分辨率匹配
+#History:
+#  20180410: init this file.
+#
+
+local_costmap:
+   global_frame: /odom_combined
+   robot_base_frame: /base_footprint
+   update_frequency: 3.0
+   publish_frequency: 2.0
+   static_map: false
+   rolling_window: true
+   width: 2.0
+   height: 2.0
+   resolution: 0.05   #should same as map.yaml file
+   transform_tolerance: 2.0
+

rosCode/src/carebot_navigation/launch/amcl_ls01d_lidar.launch

@@ -0,0 +1,69 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+   ls01d lidar to amcl, first roslaunch carebot_bringup.launch, then load map,then startup 
+   ls01d lidar to scan,then roslaunch move_base package, last roslaunch amcl package.
+  History:
+    20180408: init this file.
+    20180411: add first roslaunch carebot_bringup.launch.
+-->
+
+<launch>
+  <!-- (1) first roslaunch carebot_bringup.launch -->
+  <include file="$(find carebot_bringup)/launch/carebot_bringup.launch" />
+
+  <!-- (2) Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find carebot_navigation)/maps/mymap.yaml" />
+
+  <!-- (3) Startup leishen ls01d_lidar lidar node -->
+  <include file="$(find ls01d_lidar)/launch/ls01d_lidar.launch" />
+
+  <!-- (4) Startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <!-- (5) Run amcl -->
+  <node pkg="amcl" type="amcl" name="carebot_amcl" >
+    <param name="odom_model_type" value="omni"/> #omni wheel mobilebase
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map" />
+    <param name="odom_frame_id" value="odom_combined" />
+
+    <param name="use_map_topic" value="true" />
+    <param name="odom_alpha5" value="0.1" />
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0" />
+    <param name="laser_max_beams" value="300" />
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+
+</launch>
+

rosCode/src/carebot_navigation/launch/amcl_rplidar_a2.launch

@@ -0,0 +1,54 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find carebot_navigation)/maps/mymap.yaml" />
+
+  <!-- startup rplidar node -->
+  <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="carebot_amcl">
+    <param name="odom_model_type" value="omni"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom"/>
+
+    <param name="use_map_topic" value="true"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+
+</launch>
+

rosCode/src/carebot_navigation/launch/gazebo.launch

@@ -0,0 +1,20 @@
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find carebot_description)/urdf/carebot.urdf -urdf -model carebot"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+</launch>

rosCode/src/carebot_navigation/launch/gmapping_ls01d_lidar.launch

@@ -0,0 +1,60 @@
+<!--
+  Copyright(C): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+    gmapping with ls01d lidar, first roslaunch carebot_bringup.launch.
+  History:
+    20180411:init this file.
+-->
+
+<launch>
+  <arg name="scan_topic"  default="scan" />
+  <arg name="base_frame"  default="base_footprint"/>
+  <arg name="odom_frame"  default="odom"/>
+
+  <!-- first roslaunch carebot_bringup -->
+  <include file="$(find carebot_bringup)/launch/carebot_bringup.launch" />
+
+  <!-- second startup ls01d lidar -->
+  <include file="$(find ls01d_lidar)/launch/ls01d_lidar.launch" />
+
+  <!-- third start gmapping node -->
+  <node pkg="gmapping" type="slam_gmapping" name="simple_gmapping" output="screen">
+      <param name="base_frame" value="$(arg base_frame)"/>
+      <param name="odom_frame" value="$(arg odom_frame)"/>
+      <param name="map_update_interval" value="5.0"/>
+      <param name="maxUrange" value="8.0"/>
+      <param name="sigma" value="0.05"/>
+      <param name="kernelSize" value="1"/>
+      <param name="lstep" value="0.05"/>
+      <param name="astep" value="0.05"/>
+      <param name="iterations" value="5"/>
+      <param name="lsigma" value="0.075"/>
+      <param name="ogain" value="3.0"/>
+      <param name="lskip" value="0"/>
+      <param name="minimumScore" value="50"/>
+
+      <param name="srr" value="0.1"/>
+      <param name="srt" value="0.2"/>
+      <param name="str" value="0.1"/>
+      <param name="stt" value="0.2"/>
+
+      <param name="linearUpdate" value="0.3"/>
+      <param name="angularUpdate" value="0.4"/>
+      <param name="temporalUpdate" value="3.0"/>
+      <param name="resampleThreshold" value="0.5"/>
+      <param name="particles" value="30"/>
+
+      <param name="xmin" value="-5.0" />
+      <param name="ymin" value="-5.0" />
+      <param name="xmax" value="5.0" />
+      <param name="ymax" value="5.0" />
+      <param name="delta" value="0.05" />
+
+      <param name="llsamplerange" value="0.01" />
+      <param name="llsamplestep" value="0.01" />
+      <param name="lasamplerange" value="0.005" />
+      <param name="lasamplestep" value="0.005" />
+  </node>
+</launch>
+

rosCode/src/carebot_navigation/launch/gmapping_rplidar_a2.launch

@@ -0,0 +1,43 @@
+<launch>  
+  <param name="use_sim_time" value="false"/>  
+
+  <!-- startup rplidar node -->
+  <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find carebot_navigation)/launch/move_base.launch" />
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="map_update_interval" value="5.0"/>  
+    <param name="maxUrange" value="7.5"/>  
+    <param name="sigma" value="0.05"/>  
+    <param name="kernelSize" value="1"/>  
+    <param name="lstep" value="0.05"/>  
+    <param name="astep" value="0.05"/>  
+    <param name="iterations" value="5"/>  
+    <param name="lsigma" value="0.075"/>  
+    <param name="ogain" value="3.0"/>  
+    <param name="lskip" value="0"/>  
+    <param name="minimumScore" value="50"/>  
+    <param name="srr" value="0.1"/>  
+    <param name="srt" value="0.2"/>  
+    <param name="str" value="0.1"/>  
+    <param name="stt" value="0.2"/>  
+    <param name="linearUpdate" value="1.0"/>  
+    <param name="angularUpdate" value="0.5"/>  
+    <param name="temporalUpdate" value="3.0"/>  
+    <param name="resampleThreshold" value="0.5"/>  
+    <param name="particles" value="30"/>  
+    <param name="xmin" value="-5.0"/>  
+    <param name="ymin" value="-5.0"/>  
+    <param name="xmax" value="5.0"/>  
+    <param name="ymax" value="5.0"/>  
+    <param name="delta" value="0.05"/>  
+    <param name="llsamplerange" value="0.01"/>  
+    <param name="llsamplestep" value="0.01"/>  
+    <param name="lasamplerange" value="0.005"/>  
+    <param name="lasamplestep" value="0.005"/>  
+  </node>  
+
+</launch>  
+

rosCode/src/carebot_navigation/launch/move_base.launch

@@ -0,0 +1,26 @@
+<!--
+  Copyright(c): 2016-2018 ROS小课堂
+  Author: www.corvin.cn
+  Description:
+    move base package launch file, load config file for slam,config file list below:
+    (1)costmap_common_params.yaml -global_costmap
+    (2)costmap_common_params.yaml -local_costmap
+    (3)local_costmap_params.yaml
+    (4)global_costmap_params.yaml
+    (5)base_local_planner_params.yaml
+  History:
+    20180408:init this file.
+-->
+
+<launch>
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <rosparam file="$(find carebot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find carebot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find carebot_navigation)/config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find carebot_navigation)/config/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find carebot_navigation)/config/base_local_planner_params.yaml" command="load" />
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <remap from="odom" to="odom_ekf" />
+  </node>
+</launch>
+