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

Crash alla partenza del nodo brodcaster

Browse Source
This commit is contained in:
Emanuele
2019-11-15 14:46:16 +01:00
parent 16eed0ad4e
commit aef477e8e5
4 changed files with 50 additions and 38 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-14T17:08:03. -->
<!-- Written by QtCreator 4.9.2, 2019-11-15T14:43:44. -->
<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=786</value>
<value type="QString">SHLVL=812</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_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>
<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>
</valuemap>
<value type="int" key="ProjectExplorer.RunStepList.StepsCount">2</value>
</valuemap>

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

@@ -69,6 +69,12 @@
<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="tf_pub_rate" type="int" value="10" />
</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/" />

42
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 <tf/transform_broadcaster.h>
///////////////////////////////////////////
///////// ROS CALLBACKS //////////////////
@@ -15,8 +16,25 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
nlohmann::json c = nlohmann::json::parse(msg->data.c_str());
std::string command = c["command"];
nlohmann::json params = c["params"];
std::string action;
if (!command.compare("QUIT")){
is->isRunning = false;
} 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"].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 = deg2rad(params["frame"]["rx"].get<double>());
tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get<double>());
pos->activeFrame = twist2Pose(tempFrame).pose;
} else;
} else {
ROS_WARN("Unknown command: %s", command.c_str());
}
@@ -56,12 +74,17 @@ int main(int argc, char **argv) {
positionData pos_data;
robotData robot_data;
auxData aux_data;
rvizData rviz_data;
int tfPubRate;
/// 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));
///read parameters from server ///
@@ -70,7 +93,9 @@ int main(int argc, char **argv) {
// 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+"jointHomePosition", robot_data.joint_homes);
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
nh.getParam("/roboglue_ros_broadcaster/tf_pub_rate", tfPubRate);
@@ -78,7 +103,7 @@ int main(int argc, char **argv) {
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_INFO("Broadcaster Node Started");
/// 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),
@@ -118,6 +143,19 @@ int main(int argc, char **argv) {
}
ROS_DEBUG_ONCE("Broadcaster Node Looping");
ros::spinOnce();
/// 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");
}
/// shutdown operations
if(ros::isShuttingDown()){
declareShutdown(&publishers, &common_par);
ros::spinOnce();

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

@@ -88,22 +88,6 @@ 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"].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 = deg2rad(params["frame"]["rx"].get<double>());
tempFrame.twist.angular.y = deg2rad(params["frame"]["ry"].get<double>());
tempFrame.twist.angular.z = deg2rad(params["frame"]["rz"].get<double>());
pos->activeFrame = twist2Pose(tempFrame).pose;
}
} else if (!command.compare("RECORD")){
action = params["action"];
if (!action.compare("start")){
@@ -352,22 +336,6 @@ int main(int argc, char **argv) {
/////////// PLAY FILE ///////////////////
/////////////////////////////////////////
/// 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);
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");
}
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());