WIP #11: Imported Kevin's NavX IMU publisher

robot_high_level_control/scripts/state_machine.py

@@ -32,6 +32,7 @@ def state_idle(self):
         return
 
     def state_localizing(self):
+        # Enable camera servo node aruco_camera_centering
         return
 
     def state_drive_to_P0(self):

robot_sensors/CMakeLists.txt

@@ -45,11 +45,11 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * 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
-# )
+add_message_files(
+  FILES
+  Imu.msg
+  String.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -66,10 +66,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  sensor_msgs   std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -106,6 +106,9 @@ catkin_package(
 #  DEPENDS system_lib
 )
 
+add_executable(publisher navx/src/publisher.cpp)
+target_link_libraries(publisher ${catkin_LIBRARIES})
+
 ###########
 ## Build ##
 ###########

robot_sensors/navx/src/publisher.cpp

@@ -0,0 +1,53 @@
+#include "ros/ros.h"
+#include "./navXTimeSync/AHRS.h"
+#include "sensor_msgs/Imu.h"
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "navx");
+    ros::NodeHandle n;
+
+    ros::Publisher pub = n.advertise<std_msgs::String>("gyro", 1000);
+    ros::Rate loop_rate(10);
+
+    int count(0);
+    while (ros::ok())
+    {
+	sensor_msgs::Imu msg;
+
+	AHRS com = AHRS("/dev/ttyACM0");
+
+	msg.header.stamp = ros::Time::now();
+
+	msg.angular_velocity.x = AHRSPosUpdate.vel_x;
+	msg.angular_velocity.y = AHRSPosUpdate.vel_y;
+	msg.angular_velocity.z = AHRSPosUpdate.vel_z;
+	msg.angular_velocity_covariance = {-1, 0, 0, 0, 0, 0, 0, 0, 0};
+
+	msg.linear_acceleration.x = AHRSUpdateBase.linear_accel_x;
+	msg.linear_acceleration.y = AHRSUpdateBase.linear_accel_y;
+	msg.linear_acceleration.z = AHRSUpdateBase.linear_accel_z;
+	msg.linear_acceleration_covariance = {-1, 0, 0, 0, 0, 0, 0, 0, 0};
+
+	msg.orientation.x = AHRSUpdateBase.quat_x;
+	msg.orientation.y = AHRSUpdateBase.quat_y;
+	msg.orientation.z = AHRSUpdateBase.quat_z
+	msg.orientation_covariance = {-1, 0, 0, 0, 0, 0, 0, 0, 0};
+
+	ROS_INFO("Angular Velocity: 	(%s, %s, %s)\n
+		  Linear Acceleration:  (%s, %s, %s)\n
+		  Orientation: 		(%s, %s, %s)\n",
+		  msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z,
+		  msg.linear_acceleration.x, msg.linear_velocity.y, msg.linear_velocity.z,
+		  msg.orientation.x, msg.orientation.y, msg.orientation.z);
+
+	pub.publish(msg);
+
+	ros.spinOnce();
+
+	loop_rate.sleep();
+	++count;
+    }
+
+    return 0;
+}

robot_sensors/package.xml

@@ -18,5 +18,14 @@
   <!-- dependencies -->
   <buildtool_depend>catkin</buildtool_depend>
   <depend>rospy</depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
 
 </package>