I nodi non sono segnati come Required

Spegnimento grafecul attraverso interfaccia

roboglue_ros_ws.workspace.user

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.9.1, 2019-11-12T09:36:03. -->
+<!-- Written by QtCreator 4.9.2, 2019-11-15T17:00:10. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
@@ -72,7 +72,7 @@
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{b917fd33-cfca-4cf9-a8d9-025510846768}</value>
    <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
-   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">2</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">1</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
     <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory"></value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
@@ -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=668</value>
+     <value type="QString">SHLVL=940</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>
@@ -483,7 +483,20 @@
       <value type="QString" key="ROSProjectManager.ROSGenericStep.Target">roboglue_ros_urdriver.launch</value>
       <value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/roboglue_ros_urdriver.launch</value>
      </valuemap>
-     <value type="int" key="ProjectExplorer.RunStepList.StepsCount">1</value>
+     <valuemap type="QVariantMap" key="ProjectExplorer.RunStepList.Step.1">
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ROSProjectManager.ROSAttachToNode</value>
+      <value type="bool" key="ProjectExplorer.RunStep.Enabled">true</value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Arguments"></value>
+      <value type="QString" key="ROSProjectManager.ROSGenericStep.Command">debug</value>
+      <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.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>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
     <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>

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/config/data/test99.data

@@ -0,0 +1,51 @@
+N,X,Y,Z,rR,rP,rY,DT,DS
+N0,0.018000,0.306000,0.500000,0.000000,1.571000,1.571000,0.000000,0.000000
+N1,0.018000,0.312000,0.500000,0.000000,1.571000,1.571000,0.095493,0.006000
+N2,0.018000,0.318000,0.500000,0.000000,1.571000,1.571000,0.177313,0.006000
+N3,0.018000,0.324000,0.500000,0.000000,1.571000,1.571000,0.276251,0.006000
+N4,-0.006000,0.324000,0.500000,0.000000,1.571000,1.571000,0.376993,0.024000
+N5,-0.024000,0.312000,0.500000,0.000000,1.571000,1.571000,0.477276,0.021633
+N6,-0.030000,0.288000,0.500000,0.000000,1.571000,1.571000,0.583836,0.024739
+N7,-0.048000,0.258000,0.500000,0.000000,1.571000,1.571000,0.674398,0.034986
+N8,-0.060000,0.246000,0.500000,0.000000,1.571000,1.571000,0.779036,0.016971
+N9,-0.066000,0.240000,0.500000,0.000000,1.571000,1.571000,0.878682,0.008485
+N10,-0.078000,0.240000,0.500000,0.000000,1.571000,1.571000,0.985584,0.012000
+N11,-0.126000,0.240000,0.500000,0.000000,1.571000,1.571000,1.076938,0.048000
+N12,-0.150000,0.240000,0.500000,0.000000,1.571000,1.571000,1.173122,0.024000
+N13,-0.162000,0.252000,0.500000,0.000000,1.571000,1.571000,1.278250,0.016971
+N14,-0.174000,0.258000,0.500000,0.000000,1.571000,1.571000,1.378685,0.013416
+N15,-0.186000,0.276000,0.500000,0.000000,1.571000,1.571000,1.484213,0.021633
+N16,-0.198000,0.300000,0.500000,0.000000,1.571000,1.571000,0.006499,0.026833
+N17,-0.204000,0.324000,0.500000,0.000000,1.571000,1.571000,1.690293,0.024739
+N18,-0.204000,0.342000,0.500000,0.000000,1.571000,1.571000,1.780065,0.018000
+N19,-0.204000,0.378000,0.500000,0.000000,1.571000,1.571000,1.872321,0.036000
+N20,-0.222000,0.450000,0.500000,0.000000,1.571000,1.571000,1.977848,0.074216
+N21,-0.318000,0.468000,0.500000,0.000000,1.571000,1.571000,2.070969,0.097673
+N22,-0.396000,0.450000,0.500000,0.000000,1.571000,1.571000,2.177687,0.080050
+N23,-0.462000,0.348000,0.500000,0.000000,1.571000,1.571000,2.283137,0.121491
+N24,-0.558000,0.264000,0.500000,0.000000,1.571000,1.571000,2.392030,0.127562
+N25,-0.636000,0.246000,0.500000,0.000000,1.571000,1.571000,2.479188,0.080050
+N26,-0.702000,0.306000,0.500000,0.000000,1.571000,1.571000,2.581360,0.089196
+N27,-0.702000,0.402000,0.500000,0.000000,1.571000,1.571000,2.672705,0.096000
+N28,-0.618000,0.480000,0.500000,0.000000,1.571000,1.571000,2.780175,0.114630
+N29,-0.540000,0.468000,0.500000,0.000000,1.571000,1.571000,2.887023,0.078918
+N30,-0.300000,0.384000,0.500000,0.000000,1.571000,1.571000,2.980252,0.254275
+N31,-0.126000,0.336000,0.500000,0.000000,1.571000,1.571000,3.088443,0.180499
+N32,-0.012000,0.318000,0.500000,0.000000,1.571000,1.571000,3.172616,0.115412
+N33,0.042000,0.372000,0.500000,0.000000,1.571000,1.571000,3.277146,0.076368
+N34,0.042000,0.396000,0.500000,0.000000,1.571000,1.571000,3.385895,0.024000
+N35,0.024000,0.390000,0.500000,0.000000,1.571000,1.571000,3.478245,0.018974
+N36,0.000000,0.288000,0.500000,0.000000,1.571000,1.571000,3.585851,0.104785
+N37,0.048000,0.234000,0.500000,0.000000,1.571000,1.571000,3.673290,0.072250
+N38,0.198000,0.252000,0.500000,0.000000,1.571000,1.571000,3.786958,0.151076
+N39,0.252000,0.306000,0.500000,0.000000,1.571000,1.571000,3.881606,0.076368
+N40,0.264000,0.336000,0.500000,0.000000,1.571000,1.571000,3.977590,0.032311
+N41,0.330000,0.234000,0.500000,0.000000,1.571000,1.571000,4.082062,0.121491
+N42,0.330000,0.162000,0.500000,0.000000,1.571000,1.571000,4.177567,0.072000
+N43,0.336000,0.102000,0.500000,0.000000,1.571000,1.571000,4.284022,0.060299
+N44,0.396000,0.096000,0.500000,0.000000,1.571000,1.571000,4.377034,0.060299
+N45,0.450000,0.144000,0.500000,0.000000,1.571000,1.571000,4.494896,0.072250
+N46,0.456000,0.252000,0.500000,0.000000,1.571000,1.571000,4.604838,0.108167
+N47,0.420000,0.300000,0.500000,0.000000,1.571000,1.571000,4.684846,0.060000
+N48,0.378000,0.324000,0.500000,0.000000,1.571000,1.571000,4.774341,0.048374
+N49,0.348000,0.324000,0.500000,0.000000,1.571000,1.571000,4.876254,0.030000

src/roboglue_ros/config/meta/test99.meta

@@ -0,0 +1 @@
+{"code":"{613d5288-5d28-465d-996d-db6750a6789c}","ds":0.5,"dt":0.5,"frame_base":"---","frame_work":{"name":"---","offset":{"r":0.0,"x":0.0,"y":0.0,"z":0.0}},"n_analog":0,"n_digital":0,"n_point":50,"name":"test99","type":"time/dist"}

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,14 +58,20 @@
         <param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
     </node>
 
+    <!-- Frame Transform Broadcaster -->
+    <node name="roboglue_ros_broadcaster" pkg="roboglue_ros" type="roboglue_ros_broadcaster" output="screen" required="false">
+        <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" />
+    <node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="false">
+        <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">
+    <node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="false">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
         <param name="point_group_mode" type="string" value="dist" />
         <param name="planning_mode" type="string" value="path" />
@@ -82,7 +83,9 @@
         <param name="data_template" type="string" value="@data_template.data" />
     </node>
 
-    <!-- <param name="planning_mode" type="string" value="joint" />   -->
-    <!-- <param name="point_group_mode" type="string" value="time" /> -->
+    <!-- Start rViz API interface-->
+    <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

@@ -0,0 +1,176 @@
+/*
+20191115 - 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
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <tf/transform_broadcaster.h>
+
+///////////////////////////////////////////
+///////// ROS CALLBACKS //////////////////
+/////////////////////////////////////////
+void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata) {
+    ROS_DEBUG("Received command: [%s]", msg->data.c_str());
+    try {
+        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());
+        }
+    } catch (nlohmann::json::exception &e){
+        ROS_ERROR("Failed to parse COMMAND: [%s]", e.what());
+    }
+}
+
+void stateCallback(const std_msgs::String::ConstPtr& msg, internalState* is, auxData* aux){
+    ROS_DEBUG("Received aux I/O State Update: [%s]", msg->data.c_str());
+}
+
+void interfaceCallback(const std_msgs::String::ConstPtr& msg, internalState *is){
+    ROS_DEBUG("Received Interface Message: [%s]", msg->data.c_str());
+}
+
+void heartBeatCallback(const ros::TimerEvent& t, publisherMap* p){
+    try {
+        std_msgs::String hbMsg;
+        hbMsg.data = std::string("HB:"+
+                                 boost::replace_all_copy(ros::this_node::getName(), "/", "")+
+                                 ":" + std::to_string(t.current_real.toSec()));
+        p->find("/roboglue_com/ros2com/interface")->second->publish(hbMsg);
+    } catch (std::exception &e) {
+        ROS_ERROR("Impossible to Send HeartBeat -> %s", e.what());
+    }
+}
+
+////////////////////////////////////////
+//////////////// MAIN //////////////////
+////////////////////////////////////////
+int main(int argc, char **argv) {
+    /// node variables ///
+    COMMON_parameters common_par;
+    /// internal state struct ////
+    internalState is;
+    positionData pos_data;
+    robotData robot_data;
+    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;
+    /// set console log level ///
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
+
+    ///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);
+    // 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);
+
+    /// set spinner rate ///
+    ros::Rate loop_rate(common_par.loopRate);
+
+    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),
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
+    ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(common_par.ros_topics.stateSub, static_cast<uint32_t>(common_par.msgBufferSize),
+                                                               boost::bind(stateCallback, _1, &is, &aux_data));
+    /// advertise publish topics //
+    ros::Publisher command_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.commandPub,
+                                                                static_cast<uint32_t>(common_par.msgBufferSize));
+    ros::Publisher interface_pub = nh.advertise<std_msgs::String>(common_par.ros_topics.interfacePub,
+                                                              static_cast<uint32_t>(common_par.msgBufferSize));
+
+    /// build a list of publisher to pass to mqtt callback ///
+    publishers[command_pub.getTopic()] = &command_pub;
+    publishers[interface_pub.getTopic()] = &interface_pub;
+
+    /// load the robot model for inverse kinematics and self collision checking ///
+    /// requires parameter server and moveIT to be active
+    moveit::planning_interface::MoveGroupInterface move_group(robot_data.move_group_name);
+    robot_data.kine_model = move_group.getRobotModel();
+    robot_data.kine_state = std::make_shared<robot_state::RobotState>(robot_data.kine_model);
+    robot_data.joint_modelGroup = robot_data.kine_state->getJointModelGroup(robot_data.move_group_name);
+    robot_data.joint_names = robot_data.joint_modelGroup->getVariableNames();
+    ROS_INFO("Planning in Frame: %s for link: %s", move_group.getPlanningFrame().c_str(), move_group.getEndEffectorLink().c_str());
+    ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
+
+    /// 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) {
+        if (startCycle){
+            startCycle = false;
+            declareStartup(&publishers, &common_par);
+            ros::spinOnce();
+        }
+        ROS_DEBUG_ONCE("Broadcaster Node Looping");
+        ros::spinOnce();
+
+        /// publish planning frame transform ///
+        /// verify execution frame and set to default if needed
+        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();
+        }
+        loop_rate.sleep();
+    }
+    ros::shutdown();
+    //////////////////////////////////////////////////////
+    return 0;
+}
+////////////////////////////////////////
+///////////// END MAIN /////////////////
+////////////////////////////////////////

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

@@ -39,6 +39,7 @@
 #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>
 ///////// STANDARD LIBRARIES ////////
 #include <iostream>
 #include <fstream>
@@ -87,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"];
-                tempFrame.twist.linear.y = params["frame"]["y"];
-                tempFrame.twist.linear.z = params["frame"]["z"];
-                tempFrame.twist.angular.x = params["frame"]["rx"];
-                tempFrame.twist.angular.y = params["frame"]["ry"];
-                tempFrame.twist.angular.z = params["frame"]["rz"];
-                pos->activeFrame = twist2Pose(tempFrame).pose;
-            }
         } else if (!command.compare("RECORD")){
             action = params["action"];
             if (!action.compare("start")){
@@ -124,6 +109,13 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
                 is->isPlaying = 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());
         }
@@ -243,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;
@@ -253,10 +247,13 @@ int main(int argc, char **argv) {
     /// setup node parameters ///
     ros::init(argc, argv, "roboglue_recorder");
     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
@@ -281,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::Info);
     /// set spinner rate ///
     ros::Rate loop_rate(common_par.loopRate);
     ROS_INFO("Recorder Node Started");
@@ -328,7 +323,7 @@ int main(int argc, char **argv) {
     ROS_INFO("IK calc in Frame: %s", robot_data.kine_model->getModelFrame().c_str());
 
     /// setup rviz palnned path visualization
-    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("base_link", "rviz_visual_tools", robot_data.kine_model);
+    rviz_data.vtools = std::make_shared<mvt::MoveItVisualTools>("world", "rviz_visual_tools", robot_data.kine_model);
 
     bool startCycle = true;
     while (ros::ok() && is.isRunning){
@@ -350,9 +345,13 @@ 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){    
-            // verify execution frame and set to default if needed
-            move_group.setPoseReferenceFrame(pos_data.activeFrameName);
             // 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());
             if (i < playVector.size()){
@@ -463,7 +462,7 @@ int main(int argc, char **argv) {
 ////////////////////////////////////////
 
 bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
-    if (!is->isFileOpen){
+    if (!is->isFileOpen && !meta["name"].get<std::string>().empty()){
         // open metafile by name
         std::string metaInFilename = fd->meta_dir_name + meta["name"].get<std::string>() + fd->meta_ext;
         ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str());
@@ -531,6 +530,7 @@ bool openFile(internalState* is, fileData* fd, nlohmann::json meta){
             return false;
         }
     }
+    return false;
 }
 
 bool closeFile(internalState* is, fileData* fd){
@@ -545,7 +545,7 @@ bool closeFile(internalState* is, fileData* fd){
         }
        fd->playVect->clear();
        fd->recordVect->clear();
-    }
+    } return true;
 }
 
 bool startRecord(internalState* is, fileData* fd, nlohmann::json meta){
@@ -687,6 +687,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){
@@ -697,7 +698,7 @@ bool stopPlay(internalState* is, fileData* fd){
 }
 
 void plotPoints (rvizData* rvdata, fileData* fd){
-    if (fd->playVect != nullptr){
+    if (fd->playVect != nullptr && fd->playVect->size()>0) {
         for(auto point : *fd->playVect){
             rvdata->vtools->publishSphere(twist2Pose(point.point).pose, rvt::RED, rvt::MEDIUM);
         }
@@ -710,7 +711,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){
@@ -103,8 +105,8 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
     newPoseStamp.pose.position.x = twt.twist.linear.x;
     newPoseStamp.pose.position.z = twt.twist.linear.z;
     // FIXME ripristinare il calcolo con le coordinate di posa dal tracker, ora gli assi sono fissi
-    //qt.setRPY(twPose.twist.angular.x, twPose.twist.angular.y, twPose.twist.angular.z);
-    qt.setRPY(0, M_PI/2, M_PI/2);
+    qt.setRPY(twt.twist.angular.x, twt.twist.angular.y, twt.twist.angular.z);
+    //qt.setRPY(0, M_PI/2, M_PI/2);
     newPoseStamp.pose.orientation.x = qt.x();
     newPoseStamp.pose.orientation.y = qt.y();
     newPoseStamp.pose.orientation.z = qt.z();