def obj_response_cb(userdata, response):
if response.exists:
pose = Pose()
#Wont there be problems mixing float32 with 64? TODO
pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
#TODO, this may give problems
pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
if MANTAIN_DISTANCE_FROM_OBJECT:
pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
userdata.object_location = pose
release_pose = ObjectPose()
release_pose.pose = response.arm_coordinates
userdata.object_release_location = release_pose
print "\n\n printing pose for move to object"
print pose
print "\n\n printing pose for releasing the object"
print release_pose
print "\n that was the pose of the object\n\n"
print userdata.object_name
print "is the object name"
return succeeded
else:
userdata.object_location = None
return aborted
def _result_cb(userdata, status, result):
""" This function will analize the status of the response
If the status is aborted, will call the action again with the same
orientation, but with the distance -0,5. If the status now is succeeded,
then call the move_by with x=0.5 in front.
:type status: actionlib.GoalStatus
:type result: MoveBaseResult
"""
if status != GoalStatus.SUCCEEDED:
rospy.loginfo(C.BACKGROUND_YELLOW + "Retrying go to the target goal" + C.NATIVE_COLOR)
new_pose = pose_at_distance(self.nav_goal.target_pose.pose, DISTANCE_TO_RETRY )
self.nav_goal.target_pose.pose = new_pose
self.nav_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(move_base_action_name, MoveBaseAction, goal=self.nav_goal)
result_status = move_action.execute(userdata)
if result_status == succeeded:
new_goal = MoveBaseGoal()
new_goal.target_pose.header.frame_id = FRAME_BASE_LINK
new_goal.target_pose.pose.position.x = DISTANCE_TO_RETRY
new_goal.target_pose.header.stamp = rospy.Time.now()
move_action = SimpleActionState(MOVE_BY_ACTION_NAME , MoveBaseAction, goal=new_goal)
return move_action.execute(userdata)
def move_person(userdata, goal):
ROBOT_RADIUS = 0.5
PERSON_RADIUS = 0.5
moveGoal = MoveBaseGoal()
moveGoal.target_pose.header.stamp = rospy.Time.now()
moveGoal.target_pose.header.frame_id = 'base_link'
moveGoal.target_pose.pose.position.x = userdata.message.x
moveGoal.target_pose.pose.position.y = userdata.message.y
moveGoal.target_pose.pose = pose_at_distance(moveGoal.target_pose.pose, PERSON_RADIUS + ROBOT_RADIUS)
moveGoal.target_pose.pose.position.z = 0
orientationAngle = 0.0 # userdata.robot_position.orientation.z
#orientation vector of the robot
rV = [0, 0]
rV[0] = math.cos(orientationAngle)
rV[1] = math.sin(orientationAngle)
#orientation vector of the robot
pV = [0, 0]
pV[0] = moveGoal.target_pose.pose.position.x
pV[1] = moveGoal.target_pose.pose.position.y
#we get the cosinus of the angle with the dot product of the robot orientation vector and the vector to the person
def dot_product(v1, v2):
return sum((v1 * v2) for v1, v2 in zip(v1, v2))
def length(v):
return dot_product(v, v) ** 0.5 # sqrt(...)
def angle(v1, v2):
return math.acos(dot_product(v1, v2) / (length(v1) * length(v2)))
#3rd component of the cross product to know if the rotation is clockwise or not
clockwise = rV[0] * pV[1] - rV[1] * pV[0]
print "\n\n\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
print "robot "
print rV
print "person "
print pV
print "angle between them "
rotationAngle = angle(rV, pV)
print str(rotationAngle) + " radians " + str(rotationAngle * (180.0) / math.pi) + " degress"
if (clockwise < 0):
rotationAngle = -rotationAngle
moveGoal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, rotationAngle * 180.0 / math.pi))
print "final rotation angle "
print str(rotationAngle) + " radians " + str(rotationAngle * (180.0) / math.pi) + " degrees"
print "BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB\n\n\n"
return moveGoal
def move_to_caller_goal_cb(userdata, nav_goal):
""" Returns a MoveBaseGoal object with the position where the movement was detected
and set userdata.out_caller_position variable with a PoseStamped object """
move_base_goal = MoveBaseGoal()
move_base_goal.target_pose.header.frame_id = "/map"
pose_detected = pose_at_distance(userdata.in_goal_position, 0.5)
pose_detected.position.z = 0
teta = math.atan2(pose_detected.position.y, pose_detected.position.x)
pose_detected.orientation = Quaternion(*quaternion_from_euler(0, 0, teta))
pose = transform_pose(pose_detected, "/base_link", "/map")
move_base_goal.target_pose.pose = pose
userdata.out_caller_position = move_base_goal.target_pose
move_base_goal.target_pose.header.stamp = Time.now()
return move_base_goal
def move_person(userdata, goal):
ROBOT_RADIUS = 0.3
PERSON_RADIUS = 0.3
rospy.loginfo("POSE TYPE: "+str(type(userdata.message)))
print "Info in closest_person (find_and_go_to_person.py): ", userdata.message
pose_in_stereo = Pose()
pose_in_stereo.position = userdata.message.pos
pose_in_base_link = transform_pose(pose_in_stereo, userdata.message.header.frame_id, "/base_link")
moveGoal = MoveBaseGoal()
moveGoal.target_pose.header.stamp = rospy.Time.now()
moveGoal.target_pose.header.frame_id = "/base_link"
moveGoal.target_pose.pose.position.x = pose_in_base_link.position.x
moveGoal.target_pose.pose.position.y = pose_in_base_link.position.y
moveGoal.target_pose.pose.position.z = 0
moveGoal.target_pose.pose = pose_at_distance(moveGoal.target_pose.pose, PERSON_RADIUS + ROBOT_RADIUS)
teta = math.atan2(pose_in_base_link.position.y, pose_in_base_link.position.x)
moveGoal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, teta))
######### COMMENT BELOW IF IT DOES NOT WORK AND UNCOMMENT ABOVE #######
# orientationAngle = 0.0 # userdata.robot_position.orientation.z
# # orientation vector of the robot
# rV = [0, 0]
# rV[0] = math.cos(orientationAngle)
# rV[1] = math.sin(orientationAngle)
# #orientation vector of the robot
# pV = [0, 0]
# pV[0] = moveGoal.target_pose.pose.position.x
# pV[1] = moveGoal.target_pose.pose.position.y
# # we get the cosinus of the angle with the dot product of the robot orientation vector and the vector to the person
# def dot_product(v1, v2):
# return sum((v1 * v2) for v1, v2 in zip(v1, v2))
# def length(v):
# return dot_product(v, v) ** 0.5 # sqrt(...)
# def angle(v1, v2):
# return math.acos(dot_product(v1, v2) / (length(v1) * length(v2)))
# #3rd component of the cross product to know if the rotation is clockwise or not
# clockwise = rV[0] * pV[1] - rV[1] * pV[0]
# print "\n\n\nAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA"
# print "robot "
# print rV
# print "person "
# print pV
# print "angle between them "
# rotationAngle = angle(rV, pV)
# print str(rotationAngle) + " radians " + str(rotationAngle * (180.0) / math.pi) + " degress"
# if (clockwise < 0):
# rotationAngle = -rotationAngle
# moveGoal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, rotationAngle * 180.0 / math.pi))
# print "final rotation angle "
# print str(rotationAngle) + " radians " + str(rotationAngle * (180.0) / math.pi) + " degrees"
# print "BBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB\n\n\n"
###### COMMENT ABOVE IF IT DOES NOT WORK ##########
print "Goal in base link (find_and_go_to_person.py)", moveGoal
return moveGoal
