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
