diff --git a/diffbot_base/config/base.yaml b/diffbot_base/config/base.yaml index 73fbd89f..cf5e6371 100644 --- a/diffbot_base/config/base.yaml +++ b/diffbot_base/config/base.yaml @@ -8,5 +8,5 @@ diffbot: motor_constant: 1.0 #27.0 pwm_limit: 1.0 debug: - hardware_interface: false + hardware_interface: true base_controller: true diff --git a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h index 749e09f2..4a2691fd 100644 --- a/diffbot_base/include/diffbot_base/diffbot_hw_interface.h +++ b/diffbot_base/include/diffbot_base/diffbot_hw_interface.h @@ -9,6 +9,8 @@ #include #include #include +#include +#include // ROS Controls #include @@ -123,6 +125,11 @@ namespace diffbot_base /** \brief Callback to receive the encoder ticks from Teensy MCU */ void encoderTicksCallback(const diffbot_msgs::EncodersStamped::ConstPtr& msg_encoders); + + /** \brief Callback to receive the IMU from MCU */ + void IMUDataCallback(const diffbot_msgs::IMU::ConstPtr& msg_imu); + + void PublishIMUtoFilter(); /** \brief Callback to receive the measured joint states from Teensy MCU */ void measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states); @@ -166,7 +173,8 @@ namespace diffbot_base std::vector joint_names_; std::size_t num_joints_; urdf::Model *urdf_model_; - + // IMU message package + sensor_msgs::Imu imu; double wheel_radius_; double wheel_diameter_; double max_velocity_; @@ -201,6 +209,12 @@ namespace diffbot_base double joint_velocities_[NUM_JOINTS]; double joint_efforts_[NUM_JOINTS]; + // Data that handles IMU data from diffbot_msgs format to sensor_msgs/Imu.h + double orientation[4]; + double angular_velocity[3]; + double linear_acceleration[3]; + int IMU_data_[10]; + ros::ServiceServer srv_start_; ros::ServiceServer srv_stop_; @@ -208,10 +222,12 @@ namespace diffbot_base ros::Publisher pub_left_motor_value_; ros::Publisher pub_right_motor_value_; - // Declare publishers for angular wheel joint velocities ros::Publisher pub_wheel_cmd_velocities_; + // Declare publisher for the IMU + ros::Publisher imu_pub_; + // Declare publisher to reset the wheel encoders // used during first launch of hardware interface to avoid large difference in encoder ticks from a previous run ros::Publisher pub_reset_encoders_; @@ -222,6 +238,9 @@ namespace diffbot_base // This subscriber receives the measured angular wheel joint velocity in the custom diffbot_msgs/AngularVelocitiesStamped message ros::Subscriber sub_measured_joint_states_; + // This subscriber receives the IMU data from the Microcontroller + ros::Subscriber sub_imu_data_; + // Array to store the received encoder tick values from the \ref sub_encoder_ticks_ subscriber int encoder_ticks_[NUM_JOINTS]; JointState measured_joint_states_[NUM_JOINTS]; diff --git a/diffbot_base/launch/diffbot.launch b/diffbot_base/launch/diffbot.launch index 5e26e773..c1b62bec 100644 --- a/diffbot_base/launch/diffbot.launch +++ b/diffbot_base/launch/diffbot.launch @@ -25,4 +25,7 @@ + + + diff --git a/diffbot_base/scripts/encoders/encoders/encoders.ino b/diffbot_base/scripts/encoders/encoders/encoders.ino index c77d859f..81e9dc7c 100644 --- a/diffbot_base/scripts/encoders/encoders/encoders.ino +++ b/diffbot_base/scripts/encoders/encoders/encoders.ino @@ -97,4 +97,4 @@ void loop() { positionLeft = newLeft; positionRight = newRight; } -} +} \ No newline at end of file diff --git a/diffbot_base/scripts/encoders/encoders_imu/encoders_imu.ino b/diffbot_base/scripts/encoders/encoders_imu/encoders_imu.ino new file mode 100644 index 00000000..24252c72 --- /dev/null +++ b/diffbot_base/scripts/encoders/encoders_imu/encoders_imu.ino @@ -0,0 +1,157 @@ +/* Encoder Library - TwoKnobs Example + * http://www.pjrc.com/teensy/td_libs_Encoder.html + * + * This example code is in the public domain. + */ + + +#include +//#include +#include +#include +#include + +#include + +#include +#include +#include +#include + + +Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28); + +double orientation[4]; +double angular_velocity[3]; +double linear_acceleration[3]; + +// Encoder setup +// Change these pin numbers to the pins connected to your encoder. +// Best Performance: both pins have interrupt capability +// Good Performance: only the first pin has interrupt capability +// Low Performance: neither pin has interrupt capability +Encoder encoderLeft(5, 6); // Default pins 5, 6 +Encoder encoderRight(7, 8); // Default pins 7, 8 +// avoid using pins with LEDs attached + +ros::NodeHandle nh; + +// ROS Subscriber setup to reset both encoders to zero +void resetCallback( const std_msgs::Empty& reset) +{ + //digitalWrite(13, HIGH-digitalRead(13)); // blink the led + // reset both back to zero. + encoderLeft.write(0); + encoderRight.write(0); + nh.loginfo("Reset both wheel encoders to zero"); +} + +ros::Subscriber sub_reset("reset", resetCallback); + +// ROS Publisher setup to publish left and right encoder ticks +// This uses the custom encoder ticks message that defines an array of two integers +diffbot_msgs::Encoders encoder_ticks; +ros::Publisher pub_encoders("encoder_ticks", &encoder_ticks); + +// This uses the custom IMU imu_msg message that defines an array of 10 integers +// The first 4 integers for Orientation (4th is Z angular velocity, becuse structure) +// Next 3 integers for Acceleration +// Next 3 integers for Angular Velocity +diffbot_msgs::IMU imu_data; +ros::Publisher pub_imu("imu", &imu_data); + +void setup() +{ + + // Encoder + nh.initNode(); + nh.advertise(pub_encoders); + nh.subscribe(sub_reset); + + while (!nh.connected()) + { + nh.spinOnce(); + } + nh.loginfo("Initialize DiffBot Wheel Encoders"); + std_msgs::Empty reset; + resetCallback(reset); + delay(1); + + // IMU + nh.advertise(pub_imu); + nh.loginfo("Initialize Diffbot Axis Data"); + delay(1); + bno.begin(); + //if (!bno.begin()) + //{ + // /* There was a problem detecting the BNO055 ... check your connections */ + // Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!"); + // while (1) + // ; + //} +} + +long positionLeft = -999; +long positionRight = -999; + +void loop() { + long newLeft, newRight; + newLeft = encoderLeft.read(); + newRight = encoderRight.read(); + + encoder_ticks.ticks[0] = newLeft; + encoder_ticks.ticks[1] = newRight; + // Use at least a delay of 3 ms on the work pc and 5 ms on the Raspberry Pi + // Too low delay causes errors in rosserial similar to the following: + // [INFO]: wrong checksum for topic id and msg + // [INFO]: Wrong checksum for msg length, length 4, dropping message. + // [ERROR]: Mismatched protocol version in packet (b'\x00'): lost sync or rosserial_python is from different ros release than the rosserial client + // [INFO]: Protocol version of client is unrecognized, expected Rev 1 (rosserial 0.5+) + // [INFO]: wrong checksum for topic id and msg + // [WARN]: Last read step: message length + // [WARN]: Run loop error: Serial Port read failure: Returned short (expected 3 bytes, received 2 instead). + // [INFO]: Requesting topics... + // [ERROR]: Lost sync with device, restarting... + // [INFO]: Requesting topics... + + // Note that https://www.arduino.cc/reference/en/language/functions/time/delay/: + // Certain things do go on while the delay() function is controlling the Atmega chip, + // however, because the delay function does not disable interrupts. + // Serial communication that appears at the RX pin is recorded, PWM (analogWrite) values and pin states are maintained, + // and interrupts will work as they should. + + // IMU CODE SECTION + + // Get a new IMU sensor event // + sensors_event_t orien, accel, gyro; + bno.getEvent(&orien, Adafruit_BNO055::VECTOR_EULER); + bno.getEvent(&accel), Adafruit_BNO055::VECTOR_LINEARACCEL; + bno.getEvent(&gyro, Adafruit_BNO055::VECTOR_GYROSCOPE); + + // Bulding IMU message into array that will get publish to ROS + imu_data.imu_msg[0] = orien.orientation.x; + imu_data.imu_msg[1] = orien.orientation.y; + imu_data.imu_msg[2] = orien.orientation.z; + imu_data.imu_msg[3] = accel.acceleration.x; + imu_data.imu_msg[4] = accel.acceleration.y; + imu_data.imu_msg[5] = accel.acceleration.z; + imu_data.imu_msg[6] = gyro.gyro.x; + imu_data.imu_msg[7] = gyro.gyro.y; + imu_data.imu_msg[8] = gyro.gyro.z; + imu_data.imu_msg[9] = bno.getTemp(); + + // Publishing Nodes + pub_imu.publish(&imu_data); + pub_encoders.publish(&encoder_ticks); + nh.spinOnce(); + delay(5); + + if (newLeft != positionLeft || newRight != positionRight) + { + //String str = "Left = " + String(newLeft) + ", Right = " + String(newRight); + //nh.loginfo(str); + positionLeft = newLeft; + positionRight = newRight; + } + +} \ No newline at end of file diff --git a/diffbot_base/src/diffbot_hw_interface.cpp b/diffbot_base/src/diffbot_hw_interface.cpp index 035d3efc..c1d88bcc 100644 --- a/diffbot_base/src/diffbot_hw_interface.cpp +++ b/diffbot_base/src/diffbot_hw_interface.cpp @@ -62,12 +62,18 @@ namespace diffbot_base //Setup publisher for angular wheel joint velocity commands pub_wheel_cmd_velocities_ = nh_.advertise("wheel_cmd_velocities", 10); + // Setup publisher for the IMU format transform + imu_pub_ = nh.advertise("imu/processed", 50); + // Setup publisher to reset wheel encoders (used during first launch of the hardware interface) pub_reset_encoders_ = nh_.advertise("reset", 10); // Setup subscriber for the wheel encoders sub_encoder_ticks_ = nh_.subscribe("encoder_ticks", 10, &DiffBotHWInterface::encoderTicksCallback, this); sub_measured_joint_states_ = nh_.subscribe("measured_joint_states", 10, &DiffBotHWInterface::measuredJointStatesCallback, this); + //setup subscriber for the IMU + sub_imu_data_ = nh_.subscribe("imu", 10, &DiffBotHWInterface::IMUDataCallback, this); + // Initialize the hardware interface init(nh_, nh_); @@ -105,9 +111,10 @@ namespace diffbot_base joint_velocity_commands_[i] = 0.0; - // Initialize encoder_ticks_ to zero because receiving meaningful + // Initialize encoder_ticks_ & IMU to zero because receiving meaningful // tick values from the microcontroller might take some time encoder_ticks_[i] = 0.0; + IMU_data_[i] = 0.0; measured_joint_states_[i].angular_position_ = 0.0; measured_joint_states_[i].angular_velocity_ = 0.0; @@ -158,6 +165,9 @@ namespace diffbot_base ROS_INFO_STREAM(std::endl << ss.str()); //printState(); } + + // Publish IMU values to handle information to EKF Filter + PublishIMUtoFilter(); } void DiffBotHWInterface::write(const ros::Time& time, const ros::Duration& period) @@ -364,6 +374,55 @@ namespace diffbot_base ROS_DEBUG_STREAM_THROTTLE(1, "Right encoder ticks: " << encoder_ticks_[1]); } + /// Process updates from imu + void DiffBotHWInterface::IMUDataCallback(const diffbot_msgs::IMU::ConstPtr& msg_imu) + { + /// update current data from IMU + orientation[0] = msg_imu->imu_msg[0]; + orientation[1] = msg_imu->imu_msg[1]; + orientation[2] = msg_imu->imu_msg[2]; + orientation[3] = msg_imu->imu_msg[8]; + + angular_velocity[0] = msg_imu->imu_msg[6]; + angular_velocity[1] = msg_imu->imu_msg[7]; + angular_velocity[2] = msg_imu->imu_msg[8]; + + linear_acceleration[0] = msg_imu->imu_msg[3]; + linear_acceleration[1] = msg_imu->imu_msg[4]; + linear_acceleration[2] = msg_imu->imu_msg[5]; + + ROS_DEBUG_STREAM_THROTTLE(1, "x orientation: " << encoder_ticks_[0]); + ROS_DEBUG_STREAM_THROTTLE(1, "y orientation: " << encoder_ticks_[1]); + ROS_DEBUG_STREAM_THROTTLE(1, "z orientation: " << encoder_ticks_[2]); + ROS_DEBUG_STREAM_THROTTLE(1, "w orientation: " << encoder_ticks_[3]); + ROS_DEBUG_STREAM_THROTTLE(1, "x angular velocity: " << encoder_ticks_[0]); + ROS_DEBUG_STREAM_THROTTLE(1, "y angular velocity: " << encoder_ticks_[1]); + ROS_DEBUG_STREAM_THROTTLE(1, "z angular velocity: " << encoder_ticks_[2]); + ROS_DEBUG_STREAM_THROTTLE(1, "x linear accelaration: " << encoder_ticks_[0]); + ROS_DEBUG_STREAM_THROTTLE(1, "y linear accelaration: " << encoder_ticks_[1]); + ROS_DEBUG_STREAM_THROTTLE(1, "z linear accelaration: " << encoder_ticks_[2]); + } + + + void DiffBotHWInterface::PublishIMUtoFilter() + { + imu_pub_.publish(imu); + imu.header.stamp = ros::Time::now(); + imu.header.frame_id = "imu"; + + imu.orientation.x = orientation[0]; + imu.orientation.y = orientation[1]; + imu.orientation.z = orientation[2]; + imu.orientation.w = orientation[3]; + + imu.angular_velocity.x = angular_velocity[0]; + imu.angular_velocity.y = angular_velocity[1]; + imu.angular_velocity.z = angular_velocity[2]; + + imu.linear_acceleration.x = linear_acceleration[0]; + imu.linear_acceleration.y = linear_acceleration[1]; + imu.linear_acceleration.z = linear_acceleration[2]; + } void DiffBotHWInterface::measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states) { /// Update current encoder ticks in encoders array diff --git a/diffbot_control/config/ekf_localization.yaml b/diffbot_control/config/ekf_localization.yaml new file mode 100755 index 00000000..01ba1f50 --- /dev/null +++ b/diffbot_control/config/ekf_localization.yaml @@ -0,0 +1,66 @@ +#Configuation for robot odometry EKF +# +frequency: 50 + +two_d_mode: true + +publish_tf: false + +# Complete the frames section +odom_frame: odom +base_link_frame: base_footprint +world_frame: odom +map_frame: map + +# Complete the odom0 configuration +odom0: /diffbot/mobile_base_controller/odom +odom0_config: [false, false, false, + false, false, true, + true, true, false, + false, false, true, + false, false, false,] +odom0_differential: false + +# Complete the imu0 configuration +imu0: /diffbot/imu/processed +imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, true, + true, false, false,] +imu0_differential: true + +process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] + + +initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + + diff --git a/diffbot_control/launch/diffbot_filter.launch b/diffbot_control/launch/diffbot_filter.launch new file mode 100755 index 00000000..b44479b4 --- /dev/null +++ b/diffbot_control/launch/diffbot_filter.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/diffbot_msgs/CMakeLists.txt b/diffbot_msgs/CMakeLists.txt index 2607ade7..59e9c14b 100644 --- a/diffbot_msgs/CMakeLists.txt +++ b/diffbot_msgs/CMakeLists.txt @@ -49,6 +49,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( FILES Encoders.msg + IMU.msg EncodersStamped.msg AngularVelocities.msg AngularVelocitiesStamped.msg diff --git a/diffbot_msgs/msg/IMU.msg b/diffbot_msgs/msg/IMU.msg new file mode 100644 index 00000000..6871c38d --- /dev/null +++ b/diffbot_msgs/msg/IMU.msg @@ -0,0 +1,8 @@ +# This is a message to hold number of ticks from Encoders +Header header + +# Use an array of size two of type int32 for the two encoders. +# int32 is used instead of int64 because it is not supporte by Arduino/Teensy. +# An overflow is also unlikely with the encoders of the DG01D-E +# motor with its encoder because of its low encoder resolution +int32[10] imu_msg \ No newline at end of file