From a881191bc6d3f3036c0037cdc7503421846989e1 Mon Sep 17 00:00:00 2001
From: Derek Witcpalek <dwitcpa@umich.edu>
Date: Fri, 3 Apr 2020 20:57:47 -0400
Subject: [PATCH] Added a joint position publisher. It currently publishes all
 joint angles for the robotic arm, but could be adapted to a different model

---
 .../src/simulation_drivers/model_joints.world | 18 +++++++++++++
 .../src/simulation_env/CMakeLists.txt         | 18 ++++++-------
 .../src/simulation_env/msg/arm_angles.msg     |  6 +++++
 .../src/simulation_env/package.xml            |  2 ++
 .../src/arm_angle_sensor_plugin.cpp           | 26 ++++++++++++-------
 5 files changed, 52 insertions(+), 18 deletions(-)
 create mode 100644 rrrobot_src/src/simulation_drivers/model_joints.world
 create mode 100644 rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg

diff --git a/rrrobot_src/src/simulation_drivers/model_joints.world b/rrrobot_src/src/simulation_drivers/model_joints.world
new file mode 100644
index 0000000..dddad2a
--- /dev/null
+++ b/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>
diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt b/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt
index 910828e..3ac0389 100644
--- a/rrrobot_src/src/simulation_drivers/src/simulation_env/CMakeLists.txt
+++ b/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(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  arm_angles.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(
-#   DEPENDENCIES
-#   std_msgs
-# )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg b/rrrobot_src/src/simulation_drivers/src/simulation_env/msg/arm_angles.msg
new file mode 100644
index 0000000..a6e84c6
--- /dev/null
+++ b/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
diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml b/rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml
index 8c6680c..6efb6ec 100644
--- a/rrrobot_src/src/simulation_drivers/src/simulation_env/package.xml
+++ b/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 -->
diff --git a/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp b/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp
index 3be777b..6603639 100644
--- a/rrrobot_src/src/simulation_drivers/src/simulation_env/src/arm_angle_sensor_plugin.cpp
+++ b/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;
-      msg.data = 0.0f;
-      gazebo::physics::BasePtr joint = model->GetChild("shoulder_joint");
+      simulation_env::arm_angles msg;
+      //msg.data = 0.0f;
+      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;