Aggiunto nodo Broadcaster continuo delle trasformazioni

src/roboglue_ros/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   moveit_ros_planning
   moveit_ros_planning_interface
   moveit_visual_tools
+  tf
   )
 
 find_package (Eigen3 REQUIRED)
@@ -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}
+)
 
 
 #############

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;
+}

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.x = deg2rad(params["frame"]["rx"].get<double>());
-                tempFrame.twist.angular.y = params["frame"]["ry"].get<double>()/1000.0;
+                tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
-                tempFrame.twist.angular.z = params["frame"]["rz"].get<double>()/1000.0;
+                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");
+        }
 
         if (is.isPlaying){    
             // i -> row counter for temp vector planning in both cartesian and joint mode