From 73ab44a67e8b3a04e1a0660a38e43e55498392c8 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Fri, 15 Nov 2019 16:45:29 +0100 Subject: [PATCH] Aggiunti messaggi di debug ai nodi Crash di Recorder quando si cerca di plottare un percorso, il problema sta negli strumenti rViz, debug necessario --- roboglue_ros_ws.workspace.user | 8 ++-- .../include/roboglue_ros/roboglue_utils.h | 1 + .../launch/roboglue_ros_urdriver.launch | 25 +++++------ src/roboglue_ros/src/roboglue_broadcaster.cpp | 45 ++++++++++--------- src/roboglue_ros/src/roboglue_follower.cpp | 12 +++-- src/roboglue_ros/src/roboglue_recorder.cpp | 41 ++++++++++++----- src/roboglue_ros/src/roboglue_utils.cpp | 2 + 7 files changed, 81 insertions(+), 53 deletions(-) diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 53e2700..17d8091 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=812 + SHLVL=928 TERM=xterm _=/usr/bin/env @@ -493,8 +493,8 @@ false roboglue_ros /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros - roboglue_ros_broadcaster - /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_broadcaster + roboglue_ros_recorder + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder 2 diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 295ba72..f35ff36 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -78,6 +78,7 @@ typedef struct{ bool isRecording = false; bool isRealtime = false; bool isRunning = true; + bool setFrame = false; } internalState; ////////// POSITION DATA STORAGE /////////// diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch index b31d17e..964d892 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch @@ -13,11 +13,6 @@ - - - - - @@ -63,18 +58,18 @@ - - - - - - - + + + + + + + @@ -88,7 +83,9 @@ - - + + + + diff --git a/src/roboglue_ros/src/roboglue_broadcaster.cpp b/src/roboglue_ros/src/roboglue_broadcaster.cpp index ac1344c..d61cdfe 100644 --- a/src/roboglue_ros/src/roboglue_broadcaster.cpp +++ b/src/roboglue_ros/src/roboglue_broadcaster.cpp @@ -5,6 +5,7 @@ ////////// ROS INCLUDES ///////////// #include "ros/ros.h" #include /// utility function library +#include #include /////////////////////////////////////////// @@ -76,24 +77,22 @@ int main(int argc, char **argv) { auxData aux_data; rvizData rviz_data; int tfPubRate; + /// declare publisher map /// + publisherMap publishers; /// setup node parameters /// ros::init(argc, argv, "roboglue_broadcaster"); ros::NodeHandle nh; - publisherMap publishers; - /// variables for TF publication - tf::TransformBroadcaster tfBroadcaster; - tf::Transform planningFrame; - tf::Quaternion frameRot; - /// TODO: override default signal handler /// - // signal(SIGINT, boost::bind(sigintHandler, _1, &is)); + /// set console log level /// + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + ///read parameters from server /// nh.getParam("/roboglue_ros_broadcaster/parameter_ns", common_par.param_ns); + ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str()); loadCommonParameters(&nh, &common_par); // load robot model information 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 jog coord publish rate @@ -101,9 +100,10 @@ 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_INFO("Broadcaster Node Started"); + ROS_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str()); + ROS_DEBUG("Move Group [%s] Loaded.. ", robot_data.move_group_name.c_str()); /// subscribe to incoming topics /// ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandSub, static_cast(common_par.msgBufferSize), @@ -133,6 +133,12 @@ int main(int argc, char **argv) { /// create timed callbacks /// ros::Timer heartBeat = nh.createTimer(ros::Duration(5.0), boost::bind(heartBeatCallback, _1, &publishers)); + /// variables for TF publication + tf::TransformBroadcaster tfBroadcaster; + tf::Transform planningFrame; + tf::Quaternion frameRot; + /// setup rviz palnned path visualization + rviz_data.vtools = std::make_shared("world", "rviz_visual_tools", robot_data.kine_model); ///////////////////// MAIN LOOP ////////////////////// bool startCycle = true; while (ros::ok() && is.isRunning) { @@ -143,18 +149,17 @@ int main(int argc, char **argv) { } ROS_DEBUG_ONCE("Broadcaster Node Looping"); ros::spinOnce(); - /// publish planning frame transform /// + /// publish planning frame transform /// /// 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); - planningFrame.setRotation(frameRot); - tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame")); - rviz_data.vtools->setBaseFrame("planning_frame"); - move_group.setPoseReferenceFrame("planning_frame"); - } + 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); + planningFrame.setRotation(frameRot); + tfBroadcaster.sendTransform(tf::StampedTransform(planningFrame, ros::Time::now(), "world", "planning_frame")); + rviz_data.vtools->setBaseFrame("planning_frame"); + move_group.setPoseReferenceFrame("planning_frame"); + /// shutdown operations if(ros::isShuttingDown()){ declareShutdown(&publishers, &common_par); diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index a8285a0..d453bde 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -204,16 +204,19 @@ int main(int argc, char **argv) { auxData aux_data; std::vector joint_names; int jogPubRate; - + /// declare publisher map //// + publisherMap publishers; /// setup node parameters /// ros::init(argc, argv, "roboglue_follower"); //TODO: ros::init(argc, argv, "roboglue_follower", ros::init_options::NoSigintHandler); ros::NodeHandle nh; - publisherMap publishers; + /// set console log level /// + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); /// TODO: override default signal handler /// // signal(SIGINT, boost::bind(sigintHandler, _1, &is)); ///read parameters from server /// nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns); + ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str()); loadCommonParameters(&nh, &common_par); // load robot model information nh.getParam(common_par.param_ns+"robot_model_name", robot_data.robot_model_name); @@ -225,9 +228,10 @@ 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_INFO("Follower Node Started"); + ROS_DEBUG("Robot Model [%s] Loaded.. ", robot_data.robot_model_name.c_str()); + ROS_DEBUG("Move Group [%s] Loaded.. ", robot_data.move_group_name.c_str()); /// subscribe to incoming topics /// ros::Subscriber command_sub = nh.subscribe(common_par.ros_topics.commandSub, static_cast(common_par.msgBufferSize), diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 54ef2aa..ad62e70 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -101,13 +101,20 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p action = params["action"]; if (!action.compare("start")){ getLockInfo(pos, params["lock"]); - is->isRealtime=true; - is->isPlaying=false; - is->isRecording=false; + is->isRealtime = true; + is->isPlaying = false; + is->isRecording = false; } else if (!action.compare("stop")){ - is->isRealtime=false; - is->isPlaying=false; - is->isRecording=false; + is->isRealtime = false; + is->isPlaying = false; + is->isRecording = false; + } + } else if (!command.compare("SETFRAME")) { + action = params["action"]; + if (!action.compare("reset")){ + is->setFrame = true; + } else if (!action.compare("set")){ + is->setFrame = true; } } else { ROS_WARN("Unknown command: %s", command.c_str()); @@ -228,6 +235,8 @@ int main(int argc, char **argv) { file_data.recordVect = &recordVector; file_data.playVect = &playVector; double nonzero_move; + /// declare publisher map /// + publisherMap publishers; /// player variables /// double dsCounter, dtCounter; @@ -237,11 +246,14 @@ int main(int argc, char **argv) { size_t i=0; /// setup node parameters /// ros::init(argc, argv, "roboglue_recorder"); - ros::NodeHandle nh; - publisherMap publishers; + ros::NodeHandle nh; + /// set console log level /// + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + /// read parameters from server /// //load common parameters nh.getParam("/roboglue_ros_recorder/parameter_ns", common_par.param_ns); + ROS_DEBUG("Using Parameter Nameserver [%s]", common_par.param_ns.c_str()); loadCommonParameters(&nh, &common_par); // TODO: inserire anche questi parametri tra quelli comuni // load robot model information @@ -266,8 +278,6 @@ int main(int argc, char **argv) { nh.getParam("/roboglue_ros_recorder/point_group_mode", robot_data.point_group_mode); 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); /// set spinner rate /// ros::Rate loop_rate(common_par.loopRate); ROS_INFO("Recorder Node Started"); @@ -335,7 +345,12 @@ int main(int argc, char **argv) { ///////////////////////////////////////// /////////// PLAY FILE /////////////////// ///////////////////////////////////////// - + if (is.setFrame){ + rviz_data.vtools->setBaseFrame("planning_frame"); + move_group.setPoseReferenceFrame("planning_frame"); + rviz_data.vtools->trigger(); + is.setFrame = false; + } 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().c_str()); @@ -671,6 +686,7 @@ bool startPlay(internalState* is, fileData* fd, nlohmann::json meta){ return openFile(is,fd,meta); else if (!is->isPlaying && is->isFileOpen) return true; + else return false; } bool stopPlay(internalState* is, fileData* fd){ @@ -694,7 +710,10 @@ void plotPoints (rvizData* rvdata, fileData* fd){ } void plotTrajectory(rvizData* rvdata, robotData* robotdata, moveit_msgs::RobotTrajectory* traj) { + rvdata->vtools->setBaseFrame("world"); + rvdata->vtools->trigger(); rvdata->vtools->publishTrajectoryLine(*traj, robotdata->joint_modelGroup, rvt::colors::BLUE); + rvdata->vtools->setBaseFrame("planning_frame"); rvdata->vtools->trigger(); }; diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index fd4f7ac..a08ede5 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -6,6 +6,7 @@ /////////////// NODE MANAGEMENT /////////////////// ////////////////////////////////////////////////// bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) { + ROS_DEBUG("Node [%s] loading common parameters", ros::this_node::getName().c_str()); nh->getParam(p->param_ns+"INstate", p->ros_topics.stateSub); nh->getParam(p->param_ns+"INcommands", p->ros_topics.commandSub); nh->getParam(p->param_ns+"INcoordinates", p->ros_topics.coordSub); @@ -21,6 +22,7 @@ bool loadCommonParameters(ros::NodeHandle* nh, COMMON_parameters *p) { nh->getParam(p->param_ns+"msg_buffer", p->msgBufferSize); nh->getParam(p->param_ns+"startup_msg", p->startupMsg.data); nh->getParam(p->param_ns+"shutdown_msg", p->shutdownMsg.data); + return true; } void declareStartup(publisherMap* publishers, COMMON_parameters* common_par){