Aggiunti messaggi di debug ai nodi

Crash di Recorder quando si cerca di plottare un percorso, il problema
sta negli strumenti rViz, debug necessario

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-15T14:43:44. -->
+<!-- Written by QtCreator 4.9.2, 2019-11-15T16:41:03. -->
 <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=812</value>
+     <value type="QString">SHLVL=928</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_broadcaster</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">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>
+      <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>
      </valuemap>
      <value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
     </valuemap>

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -78,6 +78,7 @@ typedef struct{
     bool isRecording = false;
     bool isRealtime = false;
     bool isRunning = true;
+    bool setFrame = false;
 } internalState;
 
 ////////// POSITION DATA STORAGE ///////////

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -13,11 +13,6 @@
         <!-- <arg name="robot_ip" value="10.0.0.5" /> -->
     </include>
 
-    <!-- Start rViz API interface-->
-    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
-        <arg name="config" value="true" />
-    </include>
-
     <!-- Common parameter Server -->
     <group ns="roboglue_ros_common">
         <!-- ROS internal topics -->
@@ -63,18 +58,18 @@
         <param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
     </node>
 
-    <!-- Real Time Tracking -->
-    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
-        <param name="parameter_ns" type="string" value="roboglue_ros_common" />
-        <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="parameter_ns" type="string" value="/roboglue_ros_common/" />
         <param name="tf_pub_rate" type="int" value="10" />
     </node>
 
+    <!-- Real Time Tracking -->
+    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
+        <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
+        <param name="jog_pub_rate" type="int" value="5" />
+    </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/" />
@@ -88,7 +83,9 @@
         <param name="data_template" type="string" value="@data_template.data" />
     </node>
 
-    <!-- <param name="planning_mode" type="string" value="joint" />   -->
+    <!-- Start rViz API interface-->
-    <!-- <param name="point_group_mode" type="string" value="time" /> -->
+    <include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
+        <arg name="config" value="true" />
+    </include>
 
 </launch>

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 <moveit/move_group_interface/move_group_interface.h>
 #include <tf/transform_broadcaster.h>
 
 ///////////////////////////////////////////
@@ -76,24 +77,22 @@ int main(int argc, char **argv) {
     auxData aux_data;
     rvizData rviz_data;
     int tfPubRate;
+    /// declare publisher map ///
+    publisherMap publishers;
 
      /// setup node parameters ///
     ros::init(argc, argv, "roboglue_broadcaster");
     ros::NodeHandle nh;
-    publisherMap publishers;
+    /// set console log level ///
-    /// variables for TF publication
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
-    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 ///
     nh.getParam("/roboglue_ros_broadcaster/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
     loadCommonParameters(&nh, &common_par);
     // 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+"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
@@ -101,9 +100,10 @@ int main(int argc, char **argv) {
 
     /// set spinner rate ///
     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("Broadcaster Node Started");
+    ROS_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str());
+    ROS_DEBUG("Move Group  [%s] Loaded.. ", robot_data.move_group_name.c_str());
 
     /// 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),
@@ -133,6 +133,12 @@ int main(int argc, char **argv) {
     /// create timed callbacks ///
     ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0),
                                           boost::bind(heartBeatCallback, _1, &publishers));
+    /// variables for TF publication
+    tf::TransformBroadcaster tfBroadcaster;
+    tf::Transform planningFrame;
+    tf::Quaternion frameRot;
+    /// setup rviz palnned path visualization
+    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("world", "rviz_visual_tools", robot_data.kine_model);
     ///////////////////// MAIN LOOP //////////////////////
     bool startCycle = true;
     while (ros::ok() && is.isRunning) {
@@ -143,18 +149,17 @@ int main(int argc, char **argv) {
         }
         ROS_DEBUG_ONCE("Broadcaster Node Looping");
         ros::spinOnce();
-        /// publish planning frame transform ///
 
+        /// 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!");
-            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));
-            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(pos_data.activeFrame.orientation.x,pos_data.activeFrame.orientation.y,pos_data.activeFrame.orientation.z);
+        planningFrame.setRotation(frameRot);
-            planningFrame.setRotation(frameRot);
+        tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame"));
-            tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame"));
+        rviz_data.vtools->setBaseFrame("planning_frame");
-            rviz_data.vtools->setBaseFrame("planning_frame");
+        move_group.setPoseReferenceFrame("planning_frame");
-            move_group.setPoseReferenceFrame("planning_frame");
+
-        }
         /// shutdown operations
         if(ros::isShuttingDown()){
             declareShutdown(&publishers, &common_par);

src/roboglue_ros/src/roboglue_follower.cpp

@@ -204,16 +204,19 @@ int main(int argc, char **argv) {
     auxData aux_data;
     std::vector<std::string> joint_names;
     int jogPubRate;
-
+    /// declare publisher map ////
+    publisherMap publishers;
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_follower");
     //TODO: ros::init(argc, argv, "roboglue_follower", ros::init_options::NoSigintHandler);
     ros::NodeHandle nh;
-    publisherMap publishers;
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
     /// TODO: override default signal handler ///
     // signal(SIGINT, boost::bind(sigintHandler, _1, &is));
     ///read parameters from server ///
     nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
     loadCommonParameters(&nh, &common_par);
     // load robot model information
     nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name);
@@ -225,9 +228,10 @@ int main(int argc, char **argv) {
 
     /// set spinner rate ///
     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_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str());
+    ROS_DEBUG("Move Group [%s] Loaded.. ", robot_data.move_group_name.c_str());
 
     /// 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),

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -101,13 +101,20 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
             action = params["action"];
             if (!action.compare("start")){
                 getLockInfo(pos, params["lock"]);
-                is->isRealtime=true;
+                is->isRealtime = true;
-                is->isPlaying=false;
+                is->isPlaying = false;
-                is->isRecording=false;
+                is->isRecording = false;
             } else if (!action.compare("stop")){
-                is->isRealtime=false;
+                is->isRealtime = false;
-                is->isPlaying=false;
+                is->isPlaying = false;
-                is->isRecording=false;
+                is->isRecording = false;
+            }
+        } else if (!command.compare("SETFRAME")) {
+            action = params["action"];
+            if (!action.compare("reset")){
+                is->setFrame = true;
+            } else if (!action.compare("set")){
+                is->setFrame = true;
             }
         } else {
             ROS_WARN("Unknown command: %s", command.c_str());
@@ -228,6 +235,8 @@ int main(int argc, char **argv) {
     file_data.recordVect = &recordVector;
     file_data.playVect = &playVector;
     double nonzero_move;
+    /// declare publisher map ///
+    publisherMap publishers;
 
     /// player variables ///
     double dsCounter, dtCounter;
@@ -237,11 +246,14 @@ int main(int argc, char **argv) {
     size_t i=0;
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_recorder");
-    ros::NodeHandle nh;
+    ros::NodeHandle nh;    
-    publisherMap publishers;
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+
     /// read parameters from server ///
     //load common parameters
     nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns);
+    ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str());
     loadCommonParameters(&nh, &common_par);
     // TODO: inserire anche questi parametri tra quelli comuni
     // load robot model information
@@ -266,8 +278,6 @@ int main(int argc, char **argv) {
     nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode);
     nh.getParam("/roboglue_ros_recorder/planning_mode", robot_data.planning_mode);
 
-    /// set console log level ///
-    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
     /// set spinner rate ///
     ros::Rate loop_rate(common_par.loopRate);
     ROS_INFO("Recorder Node Started");
@@ -335,7 +345,12 @@ int main(int argc, char **argv) {
           /////////////////////////////////////////
          /////////// PLAY FILE ///////////////////
         /////////////////////////////////////////
-
+        if (is.setFrame){
+            rviz_data.vtools->setBaseFrame("planning_frame");
+            move_group.setPoseReferenceFrame("planning_frame");
+            rviz_data.vtools->trigger();
+            is.setFrame = false;
+        }
         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());
@@ -671,6 +686,7 @@ bool startPlay(internalState* is, fileData* fd, nlohmann::json meta){
         return openFile(is,fd,meta);
     else if (!is->isPlaying && is->isFileOpen)
         return true;
+    else return false;
 }
 
 bool stopPlay(internalState* is, fileData* fd){
@@ -694,7 +710,10 @@ void plotPoints (rvizData* rvdata, fileData* fd){
 }
 
 void plotTrajectory(rvizData* rvdata, robotData* robotdata, moveit_msgs::RobotTrajectory* traj) {
+    rvdata->vtools->setBaseFrame("world");
+    rvdata->vtools->trigger();
     rvdata->vtools->publishTrajectoryLine(*traj, robotdata->joint_modelGroup, rvt::colors::BLUE);
+    rvdata->vtools->setBaseFrame("planning_frame");
     rvdata->vtools->trigger();
 };
 

src/roboglue_ros/src/roboglue_utils.cpp

@@ -6,6 +6,7 @@
 /////////////// NODE MANAGEMENT ///////////////////
 //////////////////////////////////////////////////
 bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
+    ROS_DEBUG("Node [%s] loading common parameters", ros::this_node::getName().c_str());
     nh->getParam(p->param_ns+"INstate", p->ros_topics.stateSub);
     nh->getParam(p->param_ns+"INcommands", p->ros_topics.commandSub);
     nh->getParam(p->param_ns+"INcoordinates", p->ros_topics.coordSub);
@@ -21,6 +22,7 @@ bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) {
     nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize);
     nh->getParam(p->param_ns+"startup_msg", p->startupMsg.data);
     nh->getParam(p->param_ns+"shutdown_msg", p->shutdownMsg.data);
+    return true;
 }
 
 void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){