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

Aggiunti messaggi di debug ai nodi

Browse Source 
Crash di Recorder quando si cerca di plottare un percorso, il problema
sta negli strumenti rViz, debug necessario
This commit is contained in:
Emanuele
2019-11-15 16:45:29 +01:00
parent aef477e8e5
commit 73ab44a67e
7 changed files with 81 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
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.2, 2019-11-15T14:43:44. -->
<!-- Written by QtCreator 4.9.2, 2019-11-15T16:41: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=812</value>
<value type="QString">SHLVL=928</value>
<value type="QString">TERM=xterm</value>
<value type="QString">_=/usr/bin/env</value>
</valuelist>
@@ -493,8 +493,8 @@
<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_broadcaster</value>
<value type="QString" key="ROSProjectManager.ROSGenericStep.TargetPath">/home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_broadcaster</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>

1
src/roboglue_ros/include/roboglue_ros/roboglue_utils.h
View File

@@ -78,6 +78,7 @@ typedef struct{
bool isRecording = false;
bool isRealtime = false;
bool isRunning = true;
bool setFrame = false;
} internalState;
////////// POSITION DATA STORAGE ///////////

25
src/roboglue_ros/launch/roboglue_ros_urdriver.launch
View File

@@ -13,11 +13,6 @@
<!-- <arg name="robot_ip" value="10.0.0.5" /> -->
</include>
<!-- Start rViz API interface-->
<include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
<arg name="config" value="true" />
</include>
<!-- Common parameter Server -->
<group ns="roboglue_ros_common">
<!-- ROS internal topics -->
@@ -63,18 +58,18 @@
<param name="mqtt_OUTinterface" type="string" value="roboglue_com/ros2com/interface" />
</node>
<!-- Real Time Tracking -->
<node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
<param name="parameter_ns" type="string" value="roboglue_ros_common" />
<param name="jog_pub_rate" type="int" value="5" />
</node>
<!-- Frame Transform Broadcaster -->
<node name="roboglue_ros_broadcaster" pkg="roboglue_ros" type="roboglue_ros_broadcaster" output="screen" required="true">
<param name="parameter_ns" type="string" value="roboglue_ros_common" />
<param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
<param name="tf_pub_rate" type="int" value="10" />
</node>
<!-- Real Time Tracking -->
<node name="roboglue_ros_follower" pkg="roboglue_ros" type="roboglue_ros_follower" output="screen" required="true">
<param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
<param name="jog_pub_rate" type="int" value="5" />
</node>
<!-- Tracking data Recorder/Player -->
<node name="roboglue_ros_recorder" pkg="roboglue_ros" type="roboglue_ros_recorder" output="screen" required="true">
<param name="parameter_ns" type="string" value="/roboglue_ros_common/" />
@@ -88,7 +83,9 @@
<param name="data_template" type="string" value="@data_template.data" />
</node>
<!-- <param name="planning_mode" type="string" value="joint" /> -->
<!-- <param name="point_group_mode" type="string" value="time" /> -->
<!-- Start rViz API interface-->
<include file="$(find ur10_e_moveit_config)/launch/moveit_rviz.launch">
<arg name="config" value="true" />
</include>
</launch>

45
src/roboglue_ros/src/roboglue_broadcaster.cpp
View File

@@ -5,6 +5,7 @@
////////// ROS INCLUDES /////////////
#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>
///////////////////////////////////////////
@@ -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<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(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<mvt::MoveItVisualTools>("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);

12
src/roboglue_ros/src/roboglue_follower.cpp
View File

@@ -204,16 +204,19 @@ int main(int argc, char **argv) {
auxData aux_data;
std::vector<std::string> 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<std_msgs::String>(common_par.ros_topics.commandSub, static_cast<uint32_t>(common_par.msgBufferSize),

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

@@ -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;
@@ -238,10 +247,13 @@ int main(int argc, char **argv) {
/// setup node parameters ///
ros::init(argc, argv, "roboglue_recorder");
ros::NodeHandle nh;
publisherMap publishers;
/// 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<std::string>().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();
};

2
src/roboglue_ros/src/roboglue_utils.cpp
View File

@@ -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){