diff --git a/src/roboglue_ros/config/data/test99.data b/src/roboglue_ros/config/data/test99.data index 27ed096..5979750 100644 --- a/src/roboglue_ros/config/data/test99.data +++ b/src/roboglue_ros/config/data/test99.data @@ -1,51 +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 +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 diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 4ba76e7..adae62b 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -462,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().empty()){ // open metafile by name std::string metaInFilename = fd->meta_dir_name + meta["name"].get() + fd->meta_ext; ROS_DEBUG("Opening Meta File: [%s]", metaInFilename.c_str()); @@ -530,6 +530,7 @@ bool openFile(internalState* is, fileData* fd, nlohmann::json meta){ return false; } } + return false; } bool closeFile(internalState* is, fileData* fd){ @@ -544,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){ diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index a08ede5..79eef83 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -105,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();