diff --git a/roboglue_ros_ws.workspace.user b/roboglue_ros_ws.workspace.user index 6ec2f07..91ec781 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=194 + SHLVL=392 TERM=xterm _=/usr/bin/env @@ -550,6 +550,19 @@ ROSProjectManager.RunStepList + + + ROSProjectManager.ROSAttachToNode + true + + debug + true + roboglue_ros + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros + roboglue_ros_recorder + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_recorder + + ROSProjectManager.ROSLaunchStep @@ -562,7 +575,7 @@ roboglue_ros_urdriver_norviz.launch /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch - 1 + 2 3768 true @@ -571,7 +584,99 @@ false false - 3 + + dwarf + + cpu-cycles + + + 250 + -F + true + 4096 + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + kcachegrind + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + ROS Run Configuration + RoboGlueRUN_test + ROSProjectManager.ROSRunConfigurationThis is a test + + Run + + ROSProjectManager.RunStepList + + + + ROSProjectManager.ROSAttachToNode + true + + debug + false + roboglue_ros + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros + roboglue_ros_test + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/devel/lib/roboglue_ros/roboglue_ros_test + + + + + ROSProjectManager.ROSLaunchStep + true + + roslaunch + true + roboglue_ros + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros + test.launch + /home/emanuele/Documents/GestioneMacchine/Robot_Incollaggio/Software/roboglue_ros_ws/src/roboglue_ros/launch/test.launch + + 2 + + 3768 + false + true + false + false + true + + 4 diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch index 8cb9dd0..4d3b381 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch @@ -40,7 +40,7 @@ - + @@ -54,14 +54,14 @@ - + - + diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch index d7b450a..796b002 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch @@ -40,6 +40,7 @@ + @@ -53,12 +54,14 @@ + + diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index f25460c..c0e1dd7 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -178,6 +178,7 @@ int main(int argc, char **argv) { /// node variables /// int loopRate, msgBufferSize, jogPubRate; std::string param_ns; + std::string startup_msg_tmp; std_msgs::String startup_msg; ROS_topics ros_topics; /// internal state struct //// @@ -192,7 +193,7 @@ int main(int argc, char **argv) { ros::NodeHandle nh; ///read parameters from server /// nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns); - nh.getParam(param_ns+"startup_msg", startup_msg.data); + nh.getParam(param_ns+"startup_msg", startup_msg_tmp); nh.getParam(param_ns+"loop_rate", loopRate); nh.getParam(param_ns+"msg_buffer", msgBufferSize); nh.getParam(param_ns+"INstate", ros_topics.stateSub); @@ -266,7 +267,9 @@ int main(int argc, char **argv) { while (ros::ok() && is.isRunning) { if (startCycle){ startCycle = false; + startup_msg.data = startup_msg_tmp.c_str(); publishers[command_pub.getTopic()]->publish(startup_msg); + ros::spinOnce(); } ROS_DEBUG_ONCE("Follower Node Looping"); ros::spinOnce(); diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 881cea8..d7fb189 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -230,6 +230,7 @@ int main(int argc, char **argv) { /// player variables /// std_msgs::String startup_msg; + std::string startup_msg_tmp; double dsCounter, dtCounter; std::vector tempPlayVector; std::vector tempPlanningVector; @@ -241,7 +242,7 @@ int main(int argc, char **argv) { /// read parameters from server /// //load common parameters nh.getParam("/roboglue_ros_recorder/parameter_ns", param_ns); - nh.getParam(param_ns+"startup_msg", startup_msg.data); + nh.getParam(param_ns+"startup_msg", startup_msg_tmp); nh.getParam(param_ns+"loop_rate", loopRate); nh.getParam(param_ns+"msg_buffer", msgBufferSize); nh.getParam(param_ns+"min_nonzero_move", nonzero_move); @@ -318,7 +319,9 @@ int main(int argc, char **argv) { rviz_data.vtools->loadRemoteControl(); rviz_data.vtools->deleteAllMarkers(); rviz_data.vtools->trigger(); + startup_msg.data = startup_msg_tmp.c_str(); publishers[command_pub.getTopic()]->publish(startup_msg); + ros::spinOnce(); } ROS_DEBUG_ONCE("Recorder Node Looping"); ros::spinOnce(); diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index 20bb639..775c687 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -84,13 +84,14 @@ int main(int argc, char **argv) { std::string param_ns; MQTT_topics mqtt_topics; ROS_topics ros_topics; + std::string startup_msg_tmp; std_msgs::String startup_msg; /// setup node parameters /// ros::init(argc, argv, "roboglue_relay"); ros::NodeHandle nh; ///read parameters from server /// nh.getParam("/roboglue_ros_relay/parameter_ns", param_ns); - nh.getParam(param_ns+"startup_msg", startup_msg.data); + nh.getParam(param_ns+"startup_msg", startup_msg_tmp); nh.getParam(param_ns+"loop_rate", loopRate); nh.getParam(param_ns+"msg_buffer", msgBufferSize); nh.getParam(param_ns+"INstate", ros_topics.stateSub); @@ -154,7 +155,9 @@ int main(int argc, char **argv) { while (ros::ok()) { if (startCycle){ startCycle = false; + startup_msg.data = startup_msg_tmp.c_str(); publishers[command_pub.getTopic()]->publish(startup_msg); + ros::spinOnce(); } ROS_DEBUG_ONCE("Relay Node Looping"); ros::spinOnce(); diff --git a/src/universal_robot b/src/universal_robot index 8ea3f6a..06a251a 160000 --- a/src/universal_robot +++ b/src/universal_robot @@ -1 +1 @@ -Subproject commit 8ea3f6a1818d06a1b83d25ed93c35723c446001f +Subproject commit 06a251a9d9bbf2cc7dd80da6cf9555bd043681ff diff --git a/src/universal_robots_ros_driver b/src/universal_robots_ros_driver index 745b2c5..fd5052d 160000 --- a/src/universal_robots_ros_driver +++ b/src/universal_robots_ros_driver @@ -1 +1 @@ -Subproject commit 745b2c5fb73b4ab0dbf6aa4761f6fd6dc8fe0c79 +Subproject commit fd5052de362606cb03a159e53bc9a12e5468c6c0