def srv_get_programs_list_cb(self, req): db = dataset.connect('sqlite:////' + self.db_path) programs = db['programs'].all() resp = getProgramsListResponse() for pr in programs: resp.programs.append( message_converter.convert_dictionary_to_ros_message('art_msgs/Program', json.loads(pr['json']))) return resp
def srv_get_program_cb(self, req): db = dataset.connect('sqlite:////' + self.db_path) obj = db['programs'].find_one(program_id=req.id) if obj is None: rospy.logerr('Program ' + str(req.id) + ' does not exist in the database.') return None resp = getProgramResponse() resp.program = message_converter.convert_dictionary_to_ros_message('art_msgs/Program', json.loads(obj['json'])) return resp
def srv_get_object_cb(self, req): db = dataset.connect('sqlite:////' + self.db_path) obj = db['objects'].find_one(obj_id=req.obj_id) if obj is None: return None resp = getObjectResponse() resp.name = obj['name'] resp.model_url = obj['model_url'] resp.type = obj['type'] resp.bbox = message_converter.convert_dictionary_to_ros_message('shape_msgs/SolidPrimitive', json.loads(obj['bb'])) return resp
def convert_json_to_ros_message(message_type, json_message): """ Takes in the message type and a JSON-formatted string and returns a ROS message. Example: message_type = "std_msgs/String" json_message = '{"data": "Hello, Robot"}' ros_message = convert_json_to_ros_message(message_type, json_message) """ dictionary = json.loads(json_message) return message_converter.convert_dictionary_to_ros_message( message_type, dictionary)
def convert_json_to_ros_message(message_type, json_message, strict_mode=True): """ Takes in the message type and a JSON-formatted string and returns a ROS message. If strict_mode is set, an exception will be thrown if the json message contains extra fields Example: message_type = "std_msgs/String" json_message = '{"data": "Hello, Robot"}' ros_message = convert_json_to_ros_message(message_type, json_message) """ dictionary = json.loads(json_message) return message_converter.convert_dictionary_to_ros_message(message_type, dictionary, strict_mode=strict_mode)
def _subscribe(self, topic_name, message, type): """ Publish onto the already advertised topic the msg in the shape of a Python dict. :param str topic_name: ROS topic name. :param dict msg: Dictionary containing the definition of the message. """ msg = {'op': 'subscribe', 'topic': topic_name, 'type': type} json_msg = json.dumps(msg) self.ws.send(json_msg) json_message = self.ws.recv() dictionary = json.loads(json_message)['msg'] result = message_converter.convert_dictionary_to_ros_message( type, dictionary) #print("Type: '%s' \n Received: '%s'" % (type, result)) return result
def handle_action(self, channel, channel_type, data): if self.action_clients.get(channel['identifier'], None) is None: # Register action server and client eval("self.action_clients.update({'" + channel['identifier'] + "': actionlib.SimpleActionClient(\"" + channel['identifier'] + "\", " + channel_type + ")})", dict(globals().items() + [('self', self)])) rospy.loginfo("Waiting for '%s' action server..." % channel['identifier']) self.action_clients[channel['identifier']].wait_for_server() message_type = channel_type.replace("Action", "").replace(".msg", "").replace(".", "/") + "Goal" self.action_clients[channel['identifier']].send_goal( message_converter.convert_dictionary_to_ros_message(message_type, data)) rospy.sleep(1.0) self.action_clients[channel['identifier']].wait_for_result() state = self.action_clients[channel['identifier']].get_state() if state != 3: rospy.sleep(1.0) return self.command(data, channel) else: if not self.with_logger: data = std_msgs.msg.UInt32(0) self.coverage_publisher.publish(data) rospy.sleep(0.5) return True
def handle_topic(self, channel, data): try: identifier = channel['identifier'] message_class = [] message = message_converter.convert_dictionary_to_ros_message( channel['type'].replace(".msg", "").replace(".", "/"), data, message_class_return=message_class) if identifier not in self.publishers: self.publishers[identifier] = rospy.Publisher(identifier, message_class.pop(), queue_size=1) rospy.sleep(1) for input_config in self.inputs: if input_config['identifier'] == identifier: response_config = input_config['feedback'] publisher = self.publishers[identifier] publisher.publish(message) rospy.loginfo("Published msg:\n" + str(message)) feedback_class = roslib.message.get_message_class(response_config['type'].replace(".msg", "").replace(".", "/")) response = rospy.wait_for_message(response_config['topic'], feedback_class) rospy.loginfo("Response:\n" + str(response)) result = get_attribute(response, response_config['field']) success = response_config.get('success', result) == result or re.match(str(response_config['success']), str(result)) is not None rospy.loginfo("\nResult %s \n, success %s \n" % (str(result), str(success))) if success: if not self.with_logger: data = std_msgs.msg.UInt32(0) self.coverage_publisher.publish(data) rospy.sleep(0.5) return True else: return False except: rospy.logerr("Couldn't find feedback topic from logger config?") traceback.print_exc() return False