Added a joint position publisher. It currently publishes all joint angles for the robotic arm, but could be adapted to a different model

rrrobot_src/src/simulation_drivers/model_joints.world

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<sdf version="1.6">
+  <world name="default">
+
+    <!-- Ground Plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <include>
+      <uri>model://fanuc_robotic_arm</uri>
+    </include>
+  </world>
+</sdf>

rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
   gazebo_ros
+  message_generation
 )
 find_package(gazebo REQUIRED)
 
@@ -48,11 +49,10 @@ find_package(gazebo REQUIRED)
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+add_message_files(
-#   FILES
+  FILES
-#   Message1.msg
+  arm_angles.msg
-#   Message2.msg
+)
-# )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -69,10 +69,10 @@ find_package(gazebo REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
+generate_messages(
-#   DEPENDENCIES
+  DEPENDENCIES
-#   std_msgs
+  std_msgs
-# )
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg

@@ -0,0 +1,6 @@
+float32 shoulder_pivot_angle
+float32 shoulder_joint_angle
+float32 elbow_joint_angle
+float32 wrist_pivot_angle
+float32 wrist_joint_angle
+float32 end_effector_pivot_angle

rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml

@@ -52,12 +52,14 @@
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>gazebo_ros</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>gazebo_ros</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp

@@ -5,6 +5,7 @@
 #include <iostream>
 
 #include "ros/ros.h"
+#include "simulation_env/arm_angles.h"
 
 using std::cout;
 using std::endl;
@@ -29,7 +30,7 @@ namespace gazebo
       }
       
       nh.reset(new ros::NodeHandle("arm_node"));
-      publisher = nh->advertise<std_msgs::Float32>("arm_positions", 1);      
+      publisher = nh->advertise<simulation_env::arm_angles>("arm_positions", 1);      
     }
 
     private:
@@ -37,18 +38,25 @@ namespace gazebo
     void onUpdate()
     {
       //cout << __func__ << endl;
-      std_msgs::Float32 msg;
+      simulation_env::arm_angles msg;
-      msg.data = 0.0f;
+      //msg.data = 0.0f;
-      gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
+      msg.shoulder_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_pivot").get())->Position();
+      msg.shoulder_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("shoulder_joint").get())->Position();
+      msg.elbow_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("elbow_joint").get())->Position();
+      msg.wrist_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_pivot").get())->Position();
+      msg.wrist_joint_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("wrist_joint").get())->Position();
+      msg.end_effector_pivot_angle = dynamic_cast<gazebo::physics::Joint*>(model->GetChild("end_effector_pivot").get())->Position();
+      
+      //gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
       //cout << "Joint type: " << joint->GetType() << "joint type & EntityType::JOINT = " << (joint->GetType() & gazebo::physics::Base::EntityType::JOINT) << endl;
-      if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
+      //if((joint->GetType() & gazebo::physics::Base::EntityType::JOINT) != 0)
-      {
+      //{
         //cout << "Got an element of type JOINT" << endl;
         //cout << "Joint Position: " << dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position() << endl;
-        msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
+        //msg.data = dynamic_cast<gazebo::physics::Joint*>(joint.get())->Position();
 	//cout << "Position: " << msg.data << endl;
-      }
+      //}
-      if(msg.data != 0.0f) publisher.publish(msg);
+      /*if(msg.data != 0.0f) */publisher.publish(msg);
       ros::spinOnce();
     }
     physics::ModelPtr model;