aggiornato launch con rviz

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-05T16:47:35. -->
+<!-- Written by QtCreator 4.9.1, 2019-11-12T09:36: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=666</value>
+     <value type="QString">SHLVL=668</value>
      <value type="QString">TERM=xterm</value>
      <value type="QString">_=/usr/bin/env</value>
     </valuelist>

src/roboglue_ros/include/roboglue_ros/roboglue_utils.h

@@ -54,6 +54,7 @@ typedef struct {
     int loopRate, msgBufferSize;
     std::string param_ns;
     std_msgs::String startupMsg, shutdownMsg;
+    std::string defaultFrameName;
 } COMMON_parameters;
 
 ///////// AUX DATA STORAGE ///////////////
@@ -83,6 +84,8 @@ typedef struct{
 typedef struct {
     geometry_msgs::TwistStamped twist;
     trajectory_msgs::JointTrajectory joint;
+    geometry_msgs::Pose activeFrame;
+    std::string activeFrameName;
     bool waitIK = false;
     bool isFirst = false;
     bool isNew = false;

src/roboglue_ros/launch/roboglue_ros_urdriver.launch

@@ -1,6 +1,6 @@
 <launch>
     <!-- Set logger format -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}]: ${message}" />
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}][${line}]: ${message}" />
     <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
 
     <!-- Start ROS official URobotics controller -->
@@ -20,48 +20,58 @@
 
     <!-- Common parameter Server -->
     <group ns="roboglue_ros_common">
+        <!-- ROS internal topics -->
         <param name="INstate"   type="string" value="/roboglue_com/com2ros/state" />
         <param name="INcommands" type="string" value="/roboglue_com/com2ros/commands" />
         <param name="INcoordinates" type="string" value="/roboglue_com/com2ros/coordinates" />
+        <param name="INinterface" type="string" value="/roboglue_com/com2ros/interface" />
         <param name="OUTstate" type="string" value="/roboglue_com/ros2com/state" />
         <param name="OUTcommands" type="string" value="/roboglue_com/ros2com/commands" />
         <param name="OUTcoordinates" type="string" value="/roboglue_com/ros2com/coordinates" />
+        <param name="OUTinterface" type="string" value="/roboglue_com/ros2com/interface" />
+        <!-- ROS movement topics (For UR official driver use scalable position controller) -->
         <param name="OUTdelta_jog" type="string" value="/jog_arm_server/delta_jog_cmds" />
         <param name="OUTdelta_joint_jog" type="string" value="/jog_arm_server/delta_joint_jog_cmds" />
-        <param name="OUTtraj_jog" type="string" value="/pos_based_pos_traj_controller/command" />
+        <param name="OUTtraj_jog" type="string" value="/scaled_pos_traj_controller/command" />
+        <!-- ROS robot model description and defaults -->
         <param name="robot_model_name" type="string" value="robot_description" />
         <param name="move_group_name" type="string" value="manipulator" />
+        <param name="min_nonzero_move" type="double" value="0.005" />
+        <param name="joint_jump_tresh" type="double" value="45.0" />
+        <param name="default_frame_name" type="string" value= "/world"/>
+        <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
+        <!-- ROS node loop parameters -->
+        <param name="startup_msg" type="string" value="STARTUP"/>
+        <param name="shutdown_msg" type="string" value="SHUTDOWN"/>
+        <param name="heartbeat_msg" type="string" value="HB"/>
         <param name="loop_rate" type="int" value="100" />
         <param name="msg_buffer" type="int" value="100" />
-        <param name="joint_jump_tresh" type="double" value="45.0" />
-        <param name="min_nonzero_move" type="double" value="0.005" />
     </group>
 
     <!-- Message Relay to/from MQTT -->
     <node name="roboglue_ros_relay" pkg="roboglue_ros" type="roboglue_ros_relay" output="screen" required="true">
         <param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
-        <param name="startup_msg" type="string" value='{"STARTUP":"RELAY"}'/>
         <param name="mqtt_host" type="string" value="tcp://localhost:1883" />
         <param name="mqtt_qos" type="int" value="0" />
         <param name="mqtt_INstate" type="string" value="roboglue_com/com2ros/state" />
         <param name="mqtt_INcommands" type="string" value="roboglue_com/com2ros/commands" />
         <param name="mqtt_INcoordinates" type="string" value="roboglue_com/com2ros/coordinates" />
+        <param name="mqtt_INinterface" type="string" value="roboglue_com/com2ros/interface" />
         <param name="mqtt_OUTstate" type="string" value="roboglue_com/ros2com/state" />
         <param name="mqtt_OUTcommands" type="string" value="roboglue_com/ros2com/commands" />
         <param name="mqtt_OUTcoordinates" type="string" value="roboglue_com/ros2com/coordinates" />
+        <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="startup_msg" type="string" value='{"STARTUP":"FOLLOWER"}'/>
         <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/" />
-        <param name="startup_msg" type="string" value='{"STARTUP":"RECORDER"}'/>
         <param name="point_group_mode" type="string" value="dist" />
         <param name="planning_mode" type="string" value="path" />
         <param name="meta_dir" type="string" value="/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/config/meta/" />
@@ -72,4 +82,7 @@
         <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" /> -->
+
 </launch>

src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch

@@ -33,6 +33,7 @@
         <param name="move_group_name" type="string" value="manipulator" />
         <param name="min_nonzero_move" type="double" value="0.005" />
         <param name="joint_jump_tresh" type="double" value="45.0" />
+        <param name="default_frame_name" type="string" value= "/world"/>
         <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
         <!-- ROS node loop parameters -->
         <param name="startup_msg" type="string" value="STARTUP"/>

src/roboglue_ros/src/roboglue_follower.cpp

@@ -226,7 +226,7 @@ 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::Debug);
+    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
     ROS_INFO("Follower Node Started");
 
     /// subscribe to incoming topics ///

src/roboglue_ros/src/roboglue_recorder.cpp

@@ -87,6 +87,22 @@ 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")){
@@ -248,6 +264,8 @@ int main(int argc, char **argv) {
     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 spin rate
     nh.getParam("/roboglue_ros_recorder/loop_rate", common_par.loopRate);
     //load meta template filename
@@ -264,7 +282,7 @@ int main(int argc, char **argv) {
     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);
+    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");
@@ -305,6 +323,7 @@ int main(int argc, char **argv) {
     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();
+    move_group.setPoseReferenceFrame(pos_data.activeFrameName);
     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());
 
@@ -332,6 +351,8 @@ int main(int argc, char **argv) {
          /////////// PLAY FILE ///////////////////
         /////////////////////////////////////////
         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()){
@@ -361,7 +382,7 @@ int main(int argc, char **argv) {
                 tempPlanningVector.clear();
                 for (auto cc : tempPlayVector) {
                     tempPlanningVector.push_back(twist2Pose(cc.point));
-                    tempPlanningVector.back().header.frame_id="/world";
+                    tempPlanningVector.back().header.frame_id=pos_data.activeFrameName;
                 }
                 //TODO sincronizzazione delle uscite ausiliarie con il movimento del robot!!!
                 //TODO l'invio di traiettorie segmentato in più esecuzioni funziona

src/roboglue_ros/src/roboglue_relay.cpp

@@ -121,7 +121,7 @@ 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::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn);
     ROS_INFO("Relay Node Started");
     /// initialize MQTT client ///
     mqtt::async_client client(mqtt_topics.mqttHost,"");