From e3b88fe74262aee614a2c8d2f854a18352b261ec Mon Sep 17 00:00:00 2001
From: Emanuele <ema.trabattoni@gmail.com>
Date: Thu, 14 Nov 2019 16:10:36 +0100
Subject: [PATCH] Aggiunto nodo Broadcaster continuo delle trasformazioni

---
 src/roboglue_ros/CMakeLists.txt               | 11 ++++++-
 src/roboglue_ros/src/roboglue_broadcaster.cpp | 30 +++++++++++++++++++
 src/roboglue_ros/src/roboglue_recorder.cpp    |  9 +++---
 3 files changed, 44 insertions(+), 6 deletions(-)
 create mode 100644 src/roboglue_ros/src/roboglue_broadcaster.cpp

diff --git a/src/roboglue_ros/CMakeLists.txt b/src/roboglue_ros/CMakeLists.txt
index 3080e0e..28170b1 100644
--- a/src/roboglue_ros/CMakeLists.txt
+++ b/src/roboglue_ros/CMakeLists.txt
@@ -13,7 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_ros_planning
   moveit_ros_planning_interface
   moveit_visual_tools
-)
+  tf
+  )
 
 find_package (Eigen3 REQUIRED)
 find_package (Boost REQUIRED system thread chrono)
@@ -148,6 +149,11 @@ include_directories(
      src/roboglue_follower.cpp
      src/roboglue_utils.cpp
      )
+ add_executable(
+     ${PROJECT_NAME}_broadcaster
+     src/roboglue_broadcaster.cpp
+     src/roboglue_utils.cpp
+     )
  add_executable (
      ${PROJECT_NAME}_test
      src/roboglue_test.cpp
@@ -177,6 +183,9 @@ target_link_libraries(
 target_link_libraries(
     ${PROJECT_NAME}_follower ${catkin_LIBRARIES}
 )
+target_link_libraries(
+    ${PROJECT_NAME}_broadcaster ${catkin_LIBRARIES}
+)
 
 
 #############
diff --git a/src/roboglue_ros/src/roboglue_broadcaster.cpp b/src/roboglue_ros/src/roboglue_broadcaster.cpp
new file mode 100644
index 0000000..6b6dee9
--- /dev/null
+++ b/src/roboglue_ros/src/roboglue_broadcaster.cpp
@@ -0,0 +1,30 @@
+/*
+20190220 - Inizia la scrittura del nodo che si occupa di pubblicate le trasformazioni TF, documentazione su GIT
+*/
+
+////////// ROS INCLUDES /////////////
+#include "ros/ros.h"
+#include <roboglue_ros/roboglue_utils.h> /// utility function library
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "roboglue_broadcaster");
+    ros::NodeHandle nh;
+
+    ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
+
+    ros::Rate loop_rate(10);
+    while (ros::ok())
+    {
+        std_msgs::String msg;
+        msg.data = "hello world";
+
+        chatter_pub.publish(msg);
+
+        ros::spinOnce();
+
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp
index 9d8c8f9..239b819 100644
--- a/src/roboglue_ros/src/roboglue_recorder.cpp
+++ b/src/roboglue_ros/src/roboglue_recorder.cpp
@@ -99,9 +99,9 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
                 tempFrame.twist.linear.x = params["frame"]["x"].get<double>()/1000.0;
                 tempFrame.twist.linear.y = params["frame"]["y"].get<double>()/1000.0;
                 tempFrame.twist.linear.z = params["frame"]["z"].get<double>()/1000.0;
-                tempFrame.twist.angular.x = params["frame"]["rx"].get<double>()/1000.0;
-                tempFrame.twist.angular.y = params["frame"]["ry"].get<double>()/1000.0;
-                tempFrame.twist.angular.z = params["frame"]["rz"].get<double>()/1000.0;
+                tempFrame.twist.angular.x = deg2rad(params["frame"]["rx"].get<double>());
+                tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
+                tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get<double>());
                 pos->activeFrame = twist2Pose(tempFrame).pose;
             }
         } else if (!command.compare("RECORD")){
@@ -362,12 +362,11 @@ int main(int argc, char **argv) {
             ROS_DEBUG_ONCE("Planning Frame changed and publishing!");
             planningFrame.setOrigin(tf::Vector3(pos_data.activeFrame.position.x,pos_data.activeFrame.position.y,pos_data.activeFrame.position.z));
             frameRot.setRPY(pos_data.activeFrame.orientation.x,pos_data.activeFrame.orientation.y,pos_data.activeFrame.orientation.z);
-            frameRot.setRPY(0,0,0);
             planningFrame.setRotation(frameRot);
             tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame"));
             rviz_data.vtools->setBaseFrame("planning_frame");
+            move_group.setPoseReferenceFrame("planning_frame");
         }
-        move_group.setPoseReferenceFrame("planning_frame");
 
         if (is.isPlaying){    
             // i -> row counter for temp vector planning in both cartesian and joint mode