def execute(self, userdata): if (not isinstance(userdata.in_pose, Pose)): raise ValueError("ERROR: in_pose need be a Pose() object, not '%s'." % type(userdata.in_pose)) debugPrint("Will publish marker in: " + str(userdata.in_pose), 4) self.marker = Marker() self.marker.header.frame_id = self._marker_frame_id if self._marker_ns == "markers_namespace" and self._marker_name != "/visualization_marker": self.marker.ns = "ns_" + self._marker_name.split('/')[1] else: self.marker.ns = self._marker_ns self.marker.id = self._marker_id global marker_id self.marker.id = marker_id #set marker_id for the next call marker_id = marker_id + 1 if (marker_id >= self._mk_length): marker_id = 0 self.marker.header.stamp = rospy.Time.now() self.marker.type = SetMarkerType(self._marker_type) self.marker.action = Marker.ADD self.marker.pose = userdata.in_pose self.marker.scale.x = self._scale self.marker.scale.y = self._scale self.marker.scale.z = self._scale self.marker.color = SetColour(self._colour) self.marker.lifetime = rospy.Duration(self._marker_duration) self.publisher.publish(self.marker) debugPrint("Marker published...", 4) return succeeded
def listen_command_cb(userdata, message): debugPrint("The message listened is " + str(message), 3) grammar_tags = [tag for tag in message.tags if tag.key == 'action'] if grammar_tags and grammar_tags[0].value == follow_grammar_values[follow_grammar_name_index]: userdata.out_message_heard = follow_confirm_messages[follow_grammar_name_index] return 'command_received' return 'listen_command'
def initializeCommandVariables(userdata): global follow_grammar_name_index follow_grammar_name_index = 0 try: global bag bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag', 'w') finally: debugPrint("Odometry bag opened", 3) return succeeded
def read_odometry_cb(userdata, message): # if the value of the grammar is getout, we already heard a getin command # and we can record the odometry till we hear the getout command value = follow_grammar_values[follow_grammar_name_index] if value == 'getout': try: global bag debugPrint("Writing odometry message to the bag...", 3) bag.write('/base_odometry/odom', message) finally: debugPrint("Odometry data saved in bag", 3) return succeeded
def setPlannerParameters(userdata): debugPrint(' Setting planner parameters values for following the operator...', 4) node_to_reconfigure = "/move_by/move_base/PalLocalPlanner" client = dynamic_reconfigure.client.Client(node_to_reconfigure) params = { 'acc_lim_x': 0.65, 'acc_lim_th': 1.5, 'max_vel_x': 0.5, 'max_vel_th': 0.5} new_config = client.update_configuration(params) debugPrint("New configuration returned by the dynamic reconfigure server:\n" + str(new_config), 4) return succeeded
def execute(self, userdata): if (not isinstance(userdata.in_pose, Pose)): raise ValueError("ERROR: in_pose need be a Pose() object, not '%s'." % type(userdata.in_pose)) debugPrint("Will publish marker in: " + str(userdata.in_pose), 4) self.ellipse.pose.position.x = userdata.in_pose.position.x self.ellipse.pose.position.y = userdata.in_pose.position.y self.ellipse.pose.position.z = userdata.in_pose.position.z self.ellipse.color.a = 1.0 self.ellipse.color.r = 255 self.ellipse.color.g = 0 self.ellipse.color.b = 0 self.publisher.publish(self.ellipse) debugPrint("Marker published...", 4) return succeeded
def execute(self, userdata): if (not isinstance(userdata.in_pose, Pose)): raise ValueError("ERROR: in_pose need be a Pose() object, not '%s'." % type(userdata.in_pose)) debugPrint("Will publish marker in: " + str(userdata.in_pose), 4) self.marker = Marker() self.marker.header.frame_id = "/base_link" self.marker.ns = "markers_namespace" self.marker.id = marker_id global marker_id self.marker.id = marker_id #set marker_id for the next call marker_id = marker_id + 1 if (marker_id >= markers_length): marker_id = 0 self.marker.header.stamp = rospy.Time.now() self.marker.type = Marker.CYLINDER self.marker.action = Marker.ADD self.marker.pose.orientation.x = 0.0 self.marker.pose.orientation.y = 0.0 self.marker.pose.orientation.z = 0.0 self.marker.pose.orientation.w = 1.0 self.marker.scale.x = self._scale self.marker.scale.y = self._scale self.marker.scale.z = self._scale self.marker.pose.position.x = userdata.in_pose.position.x self.marker.pose.position.y = userdata.in_pose.position.y self.marker.pose.position.z = userdata.in_pose.position.z self.marker.color.a = 1.0 self.marker.color.r = 1.0 self.marker.color.g = random.random() self.marker.color.b = 0.0 self.marker.lifetime = rospy.Duration(MARKER_LIFETIME_DURATION) self.publisher.publish(self.marker) debugPrint("Marker published...", 4) return succeeded
def executeCommand(): value = follow_grammar_values[follow_grammar_name_index] if value == 'getin': debugPrint("Get into the elevator command received", 2) #we try to set the parameter till it doesn't throw any error while True: try: rospy.set_param("/params_learn_and_follow_operator_test/distance_to_human", ELEVATOR_DISTANCE_TO_HUMAN) break except Exception as ex: debugPrint(str(ex), 0) debugPrint("There was an error while trying to set the parameter. Trying again...", 0) elif value == 'getout': debugPrint("Get out of the elevator command received", 2) return
def setNextGrammarCallback(userdata): global follow_grammar_name_index follow_grammar_name_index += 1 debugPrint("Changing to the next grammar and set of commands", 2) if follow_grammar_name_index == len(follow_grammar_names): debugPrint("We have reached the last grammar and set of commands. Exitting from the test...", 2) global bag try: bag.close() finally: debugPrint("Odometry bag closed.", 2) return succeeded if follow_grammar_name_index == 1: return "out_grammar" return 'start_speech_again'
def listenFollowMeStartCommandCallback(userdata, message): debugPrint("The message listened is " + str(message), 3) grammar_tags = [tag for tag in message.tags if tag.key == 'action'] if grammar_tags and (grammar_tags[0].value == "stopwait" or grammar_tags[0].value == "stop"): return succeeded return aborted
def printOdometryBag(userdata): ''' bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag') odometry_marker_pub = rospy.Publisher("/track_operator/odometry_elevator", Marker) i = 0 for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']): #rospy.loginfo(str(topic) + '\n\n\n') #rospy.loginfo(str(msg) + '\n\n\n') #rospy.loginfo(str(t) + '\n\n\n') marker = Marker() marker.header.frame_id = "/odom" marker.ns = "odometry_elevator_namespace" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.pose = msg.pose.pose marker.scale.x = 0.5 marker.scale.y = 0.7 marker.scale.z = 0.9 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() rospy.loginfo("\033[00;32m" + str(marker) + "\033[m") odometry_marker_pub.publish(marker) i += 1 bag.close() ''' bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag') odometry_marker_pub = rospy.Publisher("/track_operator/baselink_to_odometry_elevator", Marker) i = 0 stack = [] for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']): #rospy.loginfo(str(topic) + '\n\n\n') #rospy.loginfo(str(msg) + '\n\n\n') #rospy.loginfo(str(t) + '\n\n\n') pose_transformed = transform_pose(msg.pose.pose, '/odom', '/base_link') if (getDebugLevel() >= 3): marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i marker.type = marker.SPHERE marker.action = marker.ADD marker.pose = pose_transformed marker.scale.x = 0.05 marker.scale.y = 0.05 marker.scale.z = 0.05 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 0.0 marker.color.b = 1.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() odometry_marker_pub.publish(marker) debugPrint("\033[01;33m" + str(marker) + "\033[m", 3) stack.append(pose_transformed) i += 1 bag.close() #TODO SHOULD WORK MOST OF THE TIMES, BUT IF THE BAG IS EMPTY OR WHITH LESS THAN 2 POSES THIS CODE WILL CRASH!!! try: initial_pose = stack.pop() final_pose = stack.pop() last_dist = euclidean_distance(initial_pose, final_pose) while (len(stack) > 0 and last_dist < 2.0): new_final_pose = stack.pop() new_dist = euclidean_distance(new_final_pose, initial_pose) if (new_dist > last_dist): last_dist = new_dist final_pose = new_final_pose except Exception as ex: debugPrint(str(ex), 0) debugPrint("The bag was empty. Setting a final pose of 2 meters backwards...", 0) initial_pose = Pose() initial_pose.position.x = 0.0 initial_pose.position.y = 0.0 final_pose = Pose() final_pose.position.x = -2.0 final_pose.position.y = 0.0 marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose = initial_pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 2.0 marker.color.a = 1.0 marker.color.r = 0.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() debugPrint(" Publishing \033[01;32m(GREEN)\033[mmarker of the INITIAL pose outside the elevator...", 3) debugPrint(rospy.loginfo("\033[01;32m" + str(marker) + "\033[m"), 3) odometry_marker_pub.publish(marker) marker = Marker() marker.header.frame_id = "/base_link" marker.ns = "baselink_to_odometry_elevator_namespace" marker.id = i + 1 marker.type = marker.CYLINDER marker.action = marker.ADD marker.pose = final_pose marker.scale.x = 0.1 marker.scale.y = 0.1 marker.scale.z = 1.0 marker.color.a = 1.0 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.lifetime = rospy.Duration(600.0) marker.header.stamp = rospy.Time.now() debugPrint(" Publishing \033[01;33m(RED)\033[mmarker of the FINAL pose outside the elevator...", 3) debugPrint(" \033[01;33m" + str(marker) + "\033[m", 3) odometry_marker_pub.publish(marker) userdata.final_pose = final_pose return succeeded