Inizia implementazione Set Frame

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/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,"");