Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Prima implementazione funzionante del cambio frame di riferimento

Browse Source
This commit is contained in:
Emanuele
2019-11-13 15:55:23 +01:00
parent b98948ce17
commit 270bf14a3e
4 changed files with 96 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
roboglue_ros_ws.workspace.user
View File

@@ -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-13T15:06:46. -->
<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=750</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>

51
src/roboglue_ros/config/data/test99.data Normal file
View File

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

1
src/roboglue_ros/config/meta/test99.meta Normal file
View File

@@ -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"}

38
src/roboglue_ros/src/roboglue_recorder.cpp
View File

@@ -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>
@@ -90,17 +91,17 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
} else if (!command.compare("SETFRAME")){
action = params["action"];
if (!action.compare("reset")){
pos->activeFrameName="/world";
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"];
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 = params["frame"]["rx"].get<double>()/1000.0;
tempFrame.twist.angular.y = params["frame"]["ry"].get<double>()/1000.0;
tempFrame.twist.angular.z = params["frame"]["rz"].get<double>()/1000.0;
pos->activeFrame = twist2Pose(tempFrame).pose;
}
} else if (!command.compare("RECORD")){
@@ -282,7 +283,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::Info);
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");
@@ -350,9 +351,24 @@ 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);
/// 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);
frameRot.setRPY(0,0,0);
planningFrame.setRotation(frameRot);
tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "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());
if (i < playVector.size()){