Crash alla partenza del nodo brodcaster

roboglue_ros_ws.workspace.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.9.2, 2019-11-14T17:08:03. -->
+<!-- Written by QtCreator 4.9.2, 2019-11-15T14:43:44. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -124,7 +124,7 @@
      <value type="QString">ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share</value>
      <value type="QString">ROS_ROOT=/opt/ros/kinetic/share/ros</value>
      <value type="QString">ROS_VERSION=1</value>
-     <value type="QString">SHLVL=786</value>
+     <value type="QString">SHLVL=812</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>
@@ -493,8 +493,8 @@
       <value type="bool" key="ROSProjectManager.ROSGenericStep.DebugContinueOnAttach">false</value>
       <value type="QString" key="ROSProjectManager.ROSGenericStep.Package">roboglue_ros</value>
       <value type="QString" key="ROSProjectManager.ROSGenericStep.PackagePath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros</value>
-      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_recorder</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_broadcaster</value>
-      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_broadcaster</value>
      </valuemap>
      <value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
     </valuemap>

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -69,6 +69,12 @@
         <param name="jog_pub_rate" type="int" value="5" />
     </node>
 
+    <!-- Frame Transform Broadcaster -->
+    <node name="roboglue_ros_broadcaster" pkg="roboglue_ros" type="roboglue_ros_broadcaster" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="roboglue_ros_common" />
+        <param name="tf_pub_rate" type="int" value="10" />
+    </node>
+
     <!-- Tracking data Recorder/Player -->
     <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />

src/roboglue_ros/src/roboglue_broadcaster.cpp

@@ -5,6 +5,7 @@
 ////////// ROS INCLUDES /////////////
 #include "ros/ros.h"
 #include <roboglue_ros/roboglue_utils.h> /// utility function library
+#include <tf/transform_broadcaster.h>
 
 ///////////////////////////////////////////
 ///////// ROS CALLBACKS //////////////////
@@ -15,8 +16,25 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
         nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
         std::string command = c["command"];
         nlohmann::json params = c["params"];
+        std::string action;
         if (!command.compare("QUIT")){
             is->isRunning = false;
+        } else if (!command.compare("SETFRAME")){
+            action = params["action"];
+            if (!action.compare("reset")){
+                pos->activeFrameName="world";
+                pos->activeFrame=geometry_msgs::Pose();
+            } else if (!action.compare("set")){
+                pos->activeFrameName = params["framename"];
+                geometry_msgs::TwistStamped tempFrame;
+                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 = 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;
         } else {
             ROS_WARN("Unknown command: %s", command.c_str());
         }
@@ -56,12 +74,17 @@ int main(int argc, char **argv) {
     positionData pos_data;
     robotData robot_data;
     auxData aux_data;
+    rvizData rviz_data;
     int tfPubRate;
 
      /// setup node parameters ///
     ros::init(argc, argv, "roboglue_broadcaster");
     ros::NodeHandle nh;
     publisherMap publishers;
+    /// variables for TF publication
+    tf::TransformBroadcaster tfBroadcaster;
+    tf::Transform planningFrame;
+    tf::Quaternion frameRot;
     /// TODO: override default signal handler ///
     // signal(SIGINT, boost::bind(sigintHandler, _1, &is));
     ///read parameters from server ///
@@ -70,7 +93,9 @@ int main(int argc, char **argv) {
     // load robot model information
     nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
     nh.getParam(common_par.param_ns+"move_group_name", robot_data.move_group_name);
-    nh.getParam(common_par.param_ns+"jointHomePosition", robot_data.joint_homes);
+    nh.getParam(common_par.param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh);
+    // load default frame parameters
+    nh.getParam(common_par.param_ns+"default_frame_name", pos_data.activeFrameName);
     // load jog coord publish rate
     nh.getParam("/roboglue_ros_broadcaster/tf_pub_rate", tfPubRate);
 
@@ -78,7 +103,7 @@ int main(int argc, char **argv) {
     ros::Rate loop_rate(common_par.loopRate);
     /// set console log level ///
     ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
-    ROS_INFO("Follower Node Started");
+    ROS_INFO("Broadcaster Node Started");
 
     /// subscribe to incoming topics ///
     ros::Subscriber command_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),
@@ -118,6 +143,19 @@ int main(int argc, char **argv) {
         }
         ROS_DEBUG_ONCE("Broadcaster Node Looping");
         ros::spinOnce();
+        /// publish planning frame transform ///
+
+        /// verify execution frame and set to default if needed
+        if (move_group.getPlanningFrame().compare(pos_data.activeFrameName)){
+            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);
+            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");
+        }
+        /// shutdown operations
         if(ros::isShuttingDown()){
             declareShutdown(&publishers, &common_par);
             ros::spinOnce();

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -88,22 +88,6 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
             } else if (!action.compare("stop")) {
                 is->isPlaying = !stopPlay(is, fd);
             }
-        } else if (!command.compare("SETFRAME")){
-            action = params["action"];
-            if (!action.compare("reset")){
-                pos->activeFrameName="world";
-                pos->activeFrame=geometry_msgs::Pose();
-            } else if (!action.compare("set")){
-                pos->activeFrameName = params["framename"];
-                geometry_msgs::TwistStamped tempFrame;
-                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 = 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")){
             action = params["action"];
             if (!action.compare("start")){
@@ -352,22 +336,6 @@ int main(int argc, char **argv) {
          /////////// PLAY FILE ///////////////////
         /////////////////////////////////////////
 
-        /// temporary variables for TF publication
-        tf::TransformBroadcaster tfBroadcaster;
-        tf::Transform planningFrame;
-        tf::Quaternion frameRot;
-
-        // verify execution frame and set to default if needed
-        if (move_group.getPlanningFrame().compare(pos_data.activeFrameName)){
-            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);
-            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
             ROS_INFO_COND(i==0,"Start Play file: [%s]", file_data.metaFile->at("name").get<std::string>().c_str());