From 15de56bf722f921900893dcd8c73664c5d9eb981 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Thu, 18 Feb 2016 20:55:10 -0600 Subject: [PATCH 1/5] Add submodule for IMU stick The IMU stick will use a submodule written for the sensor stick. --- .gitmodules | 0 ros_ws/src/arudino/imu/imu.ino | 179 --------------------------------- 2 files changed, 179 deletions(-) create mode 100644 .gitmodules delete mode 100644 ros_ws/src/arudino/imu/imu.ino diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..e69de29 diff --git a/ros_ws/src/arudino/imu/imu.ino b/ros_ws/src/arudino/imu/imu.ino deleted file mode 100644 index 85389a3..0000000 --- a/ros_ws/src/arudino/imu/imu.ino +++ /dev/null @@ -1,179 +0,0 @@ -#include - -#define GYROADDR 0x68 -#define COMPASSADDR 0x1e -#define ACCELADDR 0x53 - -union XYZBuffer { - struct { - short x,y,z; - } value; - byte buff[6]; -}; - -void changeEndian(union XYZBuffer *xyz) { - for (int i=0;i<6;i+=2) { - byte t=xyz->buff[i]; - xyz->buff[i]=xyz->buff[i+1]; - xyz->buff[i+1]=t; - } -} - -// Generically useful reading into a union type -void readXYZ(int device,union XYZBuffer *xyz) { - Wire.requestFrom(device, 6); - long start=millis(); - while (!Wire.available() && (millis()-start)<100); - if (millis()-start<100) { - for (int i=0;i<6;i++) - xyz->buff[i]=Wire.read(); - } -} - -void setupAccel(int device) { - // Check ID to see if we are communicating - Wire.beginTransmission(device); - Wire.write(0x00); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,1); - while (!Wire.available()); - byte ch=Wire.read(); - Serial.print("Accel id is 0x"); - Serial.println(ch,HEX); - // Should output E5 - - // https://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf - // Page 16 - Wire.beginTransmission(device); - Wire.write(0x2d); - Wire.write(0x08); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x38); - Wire.write(0x84); - Wire.endTransmission(); - -} -void readAccel(int device,union XYZBuffer *xyz) { - Wire.beginTransmission(device); - Wire.write(0x32); // One Reading - Wire.endTransmission(); - readXYZ(device,xyz); -} - -void setupCompass(int device) { - // Check ID to see if we are communicating - Serial.print("Compass id is "); - Wire.beginTransmission(device); - Wire.write(10); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,2); - while (!Wire.available()); - char ch=Wire.read(); - Serial.print(ch); - ch=Wire.read(); - Serial.println(ch); - // Should output H4 - -// Page 18 -// at http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf - Wire.beginTransmission(device); - Wire.write(0x00); Wire.write(0x70); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x01); Wire.write(0xA0); - Wire.endTransmission(); - Wire.beginTransmission(device); - Wire.write(0x02); Wire.write(0x00); // Reading - Wire.endTransmission(); - delay(6); -} -void readCompass(int device,union XYZBuffer *xyz) { - readXYZ(device,xyz); - changeEndian(xyz); - Wire.beginTransmission(device); - Wire.write(0x03); - Wire.endTransmission(); -} - -void setupGyro(int device) { - // Check ID to see if we are communicating - Wire.beginTransmission(device); - Wire.write(0x00); // One Reading - Wire.endTransmission(); - Wire.requestFrom(device,1); - while (!Wire.available()); - byte ch=Wire.read(); - Serial.print("Gyro id is 0x"); - Serial.println(ch,HEX); - // Should output 69 -} -void readGyro(int device,union XYZBuffer *xyz) { - // https://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf - // page 20 - Wire.beginTransmission(device); - Wire.write(0x1d); - Wire.endTransmission(); - readXYZ(device,xyz); - changeEndian(xyz); -} - -void pad(int width,int number) { - int n=abs(number); - int w=width; - if (number<0) w--; - while (n>0) { - w--; - n/=10; - } - if (number==0) w--; - for (int i=0;i Date: Sun, 21 Feb 2016 12:44:05 -0600 Subject: [PATCH 2/5] Change submodule to Robotics fork The robotics fork will allow us to modify the code to work with ros and will make it easier for us to customize the software slightly. --- .gitmodules | 3 +++ ros_ws/src/arudino/razor-9dof-ahrs | 1 + 2 files changed, 4 insertions(+) create mode 160000 ros_ws/src/arudino/razor-9dof-ahrs diff --git a/.gitmodules b/.gitmodules index e69de29..b5870a3 100644 --- a/.gitmodules +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "ros_ws/src/arudino/razor-9dof-ahrs"] + path = ros_ws/src/arudino/razor-9dof-ahrs + url = https://github.com/MST-Robotics/razor-9dof-ahrs.git diff --git a/ros_ws/src/arudino/razor-9dof-ahrs b/ros_ws/src/arudino/razor-9dof-ahrs new file mode 160000 index 0000000..34a85e3 --- /dev/null +++ b/ros_ws/src/arudino/razor-9dof-ahrs @@ -0,0 +1 @@ +Subproject commit 34a85e34defaeb1fa831277ce2669ec5bef2f866 From 939946920b6a9c03191bb0d2a3ea8f90d3bb9981 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Mon, 22 Feb 2016 15:45:17 -0600 Subject: [PATCH 3/5] Add localization package The localization package will contain code to help find the robots position. It currently houses the orientation node which is empty and simply spins forever while ros is running. A custom message has also been added which represents the euler angle orientation of the robot and has three components: yaw, pitch and roll. --- ros_ws/src/localization/CMakeLists.txt | 37 +++++++++++++++++++ ros_ws/src/localization/msg/rpy.msg | 3 ++ ros_ws/src/localization/package.xml | 26 +++++++++++++ .../src/localization/orientation_node.cpp | 21 +++++++++++ 4 files changed, 87 insertions(+) create mode 100644 ros_ws/src/localization/CMakeLists.txt create mode 100644 ros_ws/src/localization/msg/rpy.msg create mode 100644 ros_ws/src/localization/package.xml create mode 100644 ros_ws/src/localization/src/localization/orientation_node.cpp diff --git a/ros_ws/src/localization/CMakeLists.txt b/ros_ws/src/localization/CMakeLists.txt new file mode 100644 index 0000000..65a4da2 --- /dev/null +++ b/ros_ws/src/localization/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(localization) + +find_package(catkin REQUIRED COMPONENTS + roscpp + tf + message_generation + std_msgs +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ +add_message_files(FILES rpy.msg) +generate_messages(DEPENDENCIES std_msgs) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS include + LIBRARIES localization + CATKIN_DEPENDS roscpp tf message_generation std_msgs + DEPENDS system_lib +) + +########### +## Build ## +########### +include_directories( + ${catkin_INCLUDE_DIRS} + include/localization/ +) + +add_executable(orientation_node src/localization/orientation_node.cpp) +add_dependencies(orientation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(orientation_node ${catkin_LIBRARIES}) diff --git a/ros_ws/src/localization/msg/rpy.msg b/ros_ws/src/localization/msg/rpy.msg new file mode 100644 index 0000000..84885f7 --- /dev/null +++ b/ros_ws/src/localization/msg/rpy.msg @@ -0,0 +1,3 @@ +float32 roll +float32 pitch +float32 yaw \ No newline at end of file diff --git a/ros_ws/src/localization/package.xml b/ros_ws/src/localization/package.xml new file mode 100644 index 0000000..2f24f5a --- /dev/null +++ b/ros_ws/src/localization/package.xml @@ -0,0 +1,26 @@ + + + localization + 0.0.0 + The localization package + + Matt Anderson + + BSD + + http://robotics.mst.edu + https://github.com/MST-Robotics/IGVC + + Matt Anderson + + catkin + roscpp + std_msgs + message_generation + tf + roscpp + std_msgs + message_generation + tf + + \ No newline at end of file diff --git a/ros_ws/src/localization/src/localization/orientation_node.cpp b/ros_ws/src/localization/src/localization/orientation_node.cpp new file mode 100644 index 0000000..2335b93 --- /dev/null +++ b/ros_ws/src/localization/src/localization/orientation_node.cpp @@ -0,0 +1,21 @@ +/** + * @file orientation_node.cpp + * @brief The orientation node source file + */ + +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "orientation_node"); + + // Spin at 30hz + ros::Rate loop_rate(30); + + // Spin forever as long as ros is okay + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } +} \ No newline at end of file From 90219556a9bddc0349e2d2b7a46481c2d22cc2da Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Sun, 28 Feb 2016 12:54:57 -0600 Subject: [PATCH 4/5] Update IMU Submodule The submodule was update to use a custom message from localization. --- ros_ws/src/arudino/razor-9dof-ahrs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_ws/src/arudino/razor-9dof-ahrs b/ros_ws/src/arudino/razor-9dof-ahrs index 34a85e3..ce82388 160000 --- a/ros_ws/src/arudino/razor-9dof-ahrs +++ b/ros_ws/src/arudino/razor-9dof-ahrs @@ -1 +1 @@ -Subproject commit 34a85e34defaeb1fa831277ce2669ec5bef2f866 +Subproject commit ce823886f3f2380bd3bb556fc2a05e61d65c35cd From 5a589ee661b3803c5dff8fef9b5d939a4412c6d3 Mon Sep 17 00:00:00 2001 From: Matt Anderson Date: Sun, 6 Mar 2016 13:07:24 -0600 Subject: [PATCH 5/5] Add publisher to communicate with IMU A node was created that recieves messages from the imu and publishes the corresponding information as a quaternion. --- ros_ws/src/arudino/razor-9dof-ahrs | 2 +- ros_ws/src/localization/CMakeLists.txt | 3 +- .../include/localization/orientation.h | 54 +++++++++++++++++++ .../src/localization/orientation.cpp | 26 +++++++++ .../src/localization/orientation_node.cpp | 4 ++ 5 files changed, 87 insertions(+), 2 deletions(-) create mode 100644 ros_ws/src/localization/include/localization/orientation.h create mode 100644 ros_ws/src/localization/src/localization/orientation.cpp diff --git a/ros_ws/src/arudino/razor-9dof-ahrs b/ros_ws/src/arudino/razor-9dof-ahrs index ce82388..f20eea9 160000 --- a/ros_ws/src/arudino/razor-9dof-ahrs +++ b/ros_ws/src/arudino/razor-9dof-ahrs @@ -1 +1 @@ -Subproject commit ce823886f3f2380bd3bb556fc2a05e61d65c35cd +Subproject commit f20eea9d70f92035607a6953e8a6d61cf4d6b790 diff --git a/ros_ws/src/localization/CMakeLists.txt b/ros_ws/src/localization/CMakeLists.txt index 65a4da2..eb4e72d 100644 --- a/ros_ws/src/localization/CMakeLists.txt +++ b/ros_ws/src/localization/CMakeLists.txt @@ -32,6 +32,7 @@ include_directories( include/localization/ ) -add_executable(orientation_node src/localization/orientation_node.cpp) +add_executable(orientation_node src/localization/orientation_node.cpp include/localization/orientation.h + src/localization/orientation.cpp) add_dependencies(orientation_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(orientation_node ${catkin_LIBRARIES}) diff --git a/ros_ws/src/localization/include/localization/orientation.h b/ros_ws/src/localization/include/localization/orientation.h new file mode 100644 index 0000000..37f209d --- /dev/null +++ b/ros_ws/src/localization/include/localization/orientation.h @@ -0,0 +1,54 @@ +/** + * @file orientation.h + * @class Orientation + * @brief The class definition file for the Orientation class + */ + +#ifndef PROJECT_ORIENTATION_H +#define PROJECT_ORIENTATION_H + +#include +#include +#include +#include + +class Orientation +{ + private: + /** + * @brief The ros nodehandle + */ + ros::NodeHandle nh; + + /** + * @brief The subscriber to the message from the imu, expects an rpy message + */ + ros::Subscriber imu_sub; + + /** + * @brief The publisher of the imu message + */ + ros::Publisher orientation_pub; + + /** + * @brief The orientation of the robot + */ + sensor_msgs::Imu orientation; + + /** + * @brief The callback function for the rpy message + * + * This function will accept rpy messages and will publish this information to an imu message + * + * @param msg The message received from the rpy publisher + */ + void rpyCallback(const localization::rpy::ConstPtr& msg); + + public: + Orientation(); + + void update(); +}; + + +#endif //PROJECT_ORIENTATION_H diff --git a/ros_ws/src/localization/src/localization/orientation.cpp b/ros_ws/src/localization/src/localization/orientation.cpp new file mode 100644 index 0000000..f9f5696 --- /dev/null +++ b/ros_ws/src/localization/src/localization/orientation.cpp @@ -0,0 +1,26 @@ +/** + * @file orientation.cpp + * @class Orientation + * @brief The class implementation file for the orientation class + */ +#include "orientation.h" + +Orientation::Orientation() +{ + // Initialize the imu subscriber + imu_sub = nh.subscribe("imu", 1, &Orientation::rpyCallback, this); + orientation_pub = nh.advertise("orientation", 1); +} + +void Orientation::rpyCallback(const localization::rpy::ConstPtr &msg) +{ + orientation.header.stamp = ros::Time::now(); + orientation.header.frame_id = "camera_link"; + orientation.orientation = tf::createQuaternionMsgFromRollPitchYaw(msg->roll, msg->pitch, msg->yaw); +} + +void Orientation::update() +{ + orientation_pub.publish(orientation); +} + diff --git a/ros_ws/src/localization/src/localization/orientation_node.cpp b/ros_ws/src/localization/src/localization/orientation_node.cpp index 2335b93..dc9b5ed 100644 --- a/ros_ws/src/localization/src/localization/orientation_node.cpp +++ b/ros_ws/src/localization/src/localization/orientation_node.cpp @@ -4,11 +4,14 @@ */ #include +#include "orientation.h" int main(int argc, char** argv) { ros::init(argc, argv, "orientation_node"); + Orientation o; + // Spin at 30hz ros::Rate loop_rate(30); @@ -16,6 +19,7 @@ int main(int argc, char** argv) while (ros::ok()) { ros::spinOnce(); + o.update(); loop_rate.sleep(); } } \ No newline at end of file