From 9d653a5b38655592b8319383d608bb7cfbd901a8 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Tue, 14 Nov 2017 21:03:07 -0500 Subject: [PATCH 1/2] Updated instructor message publisher --- .../instructor_publish_message.py | 26 ++++++++++++++----- .../src/ros_sequential_scene_parsing.cpp | 2 ++ 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_publish_message.py b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_publish_message.py index 99cee319..cb0dd65e 100644 --- a/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_publish_message.py +++ b/costar_instructor/instructor_plugins/src/instructor_plugins/instructor_publish_message.py @@ -17,7 +17,8 @@ import tf_conversions as tf_c # Driver services for ur5 #from robotiq_c_model_control.srv import * -from std_srvs.srv import Empty +# from std_srvs.srv import Empty +import os colors = ColorOptions().colors # Node Wrappers ----------------------------------------------------------- @@ -28,7 +29,14 @@ def __init__(self): self.title.setStyleSheet('background-color:'+colors['purple'].normal+';color:#ffffff') self.wait_finish = NamedField('Wait', '','purple') self.wait_finish.set_field('1') - self.note = NoteField('(1 = true, 0 = false)','purple') + + self.message_contents = NamedField('message','', 'purple') + self.message_topic = NamedField('Rostopic','', 'purple') + self.message_topic.set_field('info') + + self.note = NoteField('(1 = true, 0 = false)\nrostopic has prefix: /costar/messages/','purple') + self.layout_.addWidget(self.message_contents) + self.layout_.addWidget(self.message_topic) self.layout_.addWidget(self.wait_finish) self.layout_.addWidget(self.note) @@ -38,18 +46,22 @@ def load_data(self,data): pass def generate(self): if all([self.name.full(),self.wait_finish.full()]): - return NodeActionPublishMessage(self.get_name(),self.get_label(),int(self.wait_finish.get())) + full_topic_name = os.path.join('/costar/messages/',str(self.message_topic.get())) + return NodeActionPublishMessage(self.get_name(),str(self.message_contents.get()), full_topic_name, int(self.wait_finish.get())) else: rospy.logerr('check that all menu items are properly selected for this node') return 'ERROR: check that all menu items are properly selected for this node' # Nodes ------------------------------------------------------------------- class NodeActionPublishMessage(ServiceNode): - def __init__(self,name,label,wait_finish): - L = "Publish Message: " + label + def __init__(self,name,message_contents,topic_name,wait_finish): + L = "Publish Message: " + message_contents + "\nto " + topic_name super(NodeActionPublishMessage,self).__init__(name,L,colors['purple'].normal,"Message Server") - self.message_pub = rospy.Publisher('/instructor/message_server', String,queue_size=100) - self.message_to_publish = label + # if topic_name == '': + # self.message_pub = rospy.Publisher('/instructor/message_server', String,queue_size=100) + # else: + self.message_pub = rospy.Publisher(topic_name, String,queue_size=100) + self.message_to_publish = message_contents if wait_finish == 0: self.wait_finish = False else: diff --git a/costar_perception/sequential_scene_parsing/src/ros_sequential_scene_parsing.cpp b/costar_perception/sequential_scene_parsing/src/ros_sequential_scene_parsing.cpp index 4b97405f..f6b7d5b7 100644 --- a/costar_perception/sequential_scene_parsing/src/ros_sequential_scene_parsing.cpp +++ b/costar_perception/sequential_scene_parsing/src/ros_sequential_scene_parsing.cpp @@ -381,6 +381,7 @@ void RosSceneHypothesisAssessor::processDetectedObjectMsgs() this->scene_graph_pub.publish(this->generateSceneGraphMsgs()); std_msgs::Empty done_msg; this->done_message_pub.publish(done_msg); + std::cerr << "Published done message.\n"; } this->mtx_.unlock(); @@ -463,6 +464,7 @@ void RosSceneHypothesisAssessor::processHypotheses() std_msgs::Empty done_msg; this->done_message_pub.publish(done_msg); + std::cerr << "Published done message.\n"; this->scene_graph_pub.publish(this->generateSceneGraphMsgs()); From 4b56af040d1a7c74038e48ef1ff7177ecbe8e24d Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Wed, 15 Nov 2017 12:51:02 -0500 Subject: [PATCH 2/2] update generate message --- costar_tools/moveit_collision_environment/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/costar_tools/moveit_collision_environment/CMakeLists.txt b/costar_tools/moveit_collision_environment/CMakeLists.txt index 4a1c9a21..29d7493b 100755 --- a/costar_tools/moveit_collision_environment/CMakeLists.txt +++ b/costar_tools/moveit_collision_environment/CMakeLists.txt @@ -155,6 +155,7 @@ target_link_libraries(planning_scene_generator collision_env ${catkin_LIBRARIES} ## Declare a C++ executable add_executable(moveit_collision_environment src/main_collision_env.cpp) +add_dependencies(moveit_collision_environment moveit_collision_environment_generate_messages_cpp) ## Add cmake target dependencies of the executable ## same as for the library above