From b7f1857e5c29ddc324795848a7219f2b676bd57a Mon Sep 17 00:00:00 2001 From: Emanuele Date: Tue, 12 Nov 2019 10:51:15 +0100 Subject: [PATCH] Inizia implementazione Set Frame --- roboglue_ros_ws.workspace.user | 4 +-- .../include/roboglue_ros/roboglue_utils.h | 3 +++ src/roboglue_ros/src/roboglue_follower.cpp | 2 +- src/roboglue_ros/src/roboglue_recorder.cpp | 25 +++++++++++++++++-- src/roboglue_ros/src/roboglue_relay.cpp | 2 +- 5 files changed, 30 insertions(+), 6 deletions(-) diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index c342b63..bbb0d04 100644 --- a/roboglue_ros_ws.workspace.user +++ b/roboglue_ros_ws.workspace.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -124,7 +124,7 @@ ROS_PACKAGE_PATH=/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src:/opt/ros/kinetic/share ROS_ROOT=/opt/ros/kinetic/share/ros ROS_VERSION=1 - SHLVL=666 + SHLVL=668 TERM=xterm _=/usr/bin/env diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 578c46d..295ba72 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/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; diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 8646523..a8285a0 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/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 /// diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 00509f9..5b351c4 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/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_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().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 diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index d61eee1..1ebe2d6 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/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,"");