Quaternion

.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

ros_ws/src/localization/CMakeLists.txt

@@ -0,0 +1,38 @@
+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 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})

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 <ros/ros.h>
+#include <localization/rpy.h>
+#include <sensor_msgs/Imu.h>
+#include <tf/transform_datatypes.h>
+
+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

ros_ws/src/localization/msg/rpy.msg

@@ -0,0 +1,3 @@
+float32 roll
+float32 pitch
+float32 yaw

ros_ws/src/localization/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package>
+  <name>localization</name>
+  <version>0.0.0</version>
+  <description>The localization package</description>
+
+  <maintainer email="mia2n4@mst.edu">Matt Anderson</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://robotics.mst.edu</url>
+  <url type="repository">https://github.com/MST-Robotics/IGVC</url>
+
+  <author email="mia2n4@mst.edu">Matt Anderson</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_generation</run_depend>
+  <run_depend>tf</run_depend>
+
+</package>

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<localization::rpy>("imu", 1, &Orientation::rpyCallback, this);
+    orientation_pub = nh.advertise<sensor_msgs::Imu>("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);
+}
+

ros_ws/src/localization/src/localization/orientation_node.cpp

@@ -0,0 +1,25 @@
+/**
+ * @file orientation_node.cpp
+ * @brief The orientation node source file
+ */
+
+#include <ros/ros.h>
+#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);
+
+    // Spin forever as long as ros is okay
+    while (ros::ok())
+    {
+        ros::spinOnce();
+        o.update();
+        loop_rate.sleep();
+    }
+}