def __init__(self, behavior_name, interrupt_var, move_base_service_str):
super(TrashcanEmptyingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
(self.trolley_position_, self.trashcan_position_) = (None, None)
def __init__(self, behavior_name, interrupt_var, move_base_service_str, map_accessibility_service_str, clean_pattern_str):
super(DirtRemovingBehavior, self).__init__(behavior_name, interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior", self.interrupt_var_, move_base_service_str)
self.dirt_position_ = None
self.map_accessibility_service_str_ = map_accessibility_service_str
self.clean_pattern_str_ = clean_pattern_str
self.use_vacuum_cleaner_ = False
def __init__(self, behavior_name, interrupt_var, move_base_service_str):
super(TrashcanEmptyingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
(self.trolley_pose_, self.trashcan_pose_) = (None, None)
(self.trolley_boundingbox_, self.trashcan_boundingbox_) = (None, None)
self.catch_trashcan_service_str_ = srv.CATCH_TRASHCAN_SERVICE_STR
self.empty_trashcan_service_str_ = srv.EMPTY_TRASHCAN_SERVICE_STR
self.leave_trashcan_service_str_ = srv.LEAVE_TRASHCAN_SERVICE_STR
self.transport_position_service_str_ = srv.TRANSPORT_POSITION_STR
self.rest_position_service_str_ = srv.REST_POSITION_STR
self.map_accessibility_service_str_ = srv.MAP_ACCESSIBILITY_SERVICE_STR
class DirtRemovingBehavior(behavior_container.BehaviorContainer):
# ========================================================================
# Description:
# Class which contains the behavior for removing dirt spots
# > Go to the dirt location
# > Clean
# ========================================================================
def __init__(self, behavior_name, interrupt_var, move_base_service_str,
map_accessibility_service_str, clean_pattern_str):
super(DirtRemovingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
self.dirt_position_ = None
self.map_accessibility_service_str_ = map_accessibility_service_str
self.clean_pattern_str_ = clean_pattern_str
self.use_vacuum_cleaner_ = False
# Method for setting parameters for the behavior
def setParameters(self, dirt_position):
self.dirt_position_ = dirt_position
# Method for returning to the standard pose of the robot
def returnToRobotStandardState(self):
# save current data if necessary
# undo or check whether everything has been undone
pass
def computeAccessiblePosesForVacuumCleaner(self):
accessibility_checker = rospy.ServiceProxy(
self.map_accessibility_service_str_, CheckPerimeterAccessibility)
request = CheckPerimeterAccessibilityRequest()
center = Pose2D()
(center.x, center.y) = (self.dirt_position_.x, self.dirt_position_.y)
center.theta = 0
request.center = center
request.radius = 0.8 # todo (rmb-ma) use the value from the cleaning device
request.rotational_sampling_step = 0.3
response = accessibility_checker(request)
accessible_poses = response.accessible_poses_on_perimeter
return accessible_poses
@staticmethod
def computeBestPoseForRobot(accessible_locations):
assert (len(accessible_locations) > 0)
(robot_position, robot_rotation, _) = getCurrentRobotPosition()
(x_robot, y_robot) = (robot_position[0], robot_position[1])
min_dist = float('inf')
for pose in accessible_locations:
(x, y) = (pose.x, pose.y)
current_dist = (x - x_robot)**2 + (y - y_robot)**2
if current_dist < min_dist:
min_dist = current_dist
best_pose2d = pose
best_pose3d = Pose()
(best_pose3d.position.x, best_pose3d.position.y) = (best_pose2d.x,
best_pose2d.y)
best_pose3d.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, best_pose2d.theta))
return best_pose3d
@staticmethod
def normalize(angle):
return atan2(sin(angle), cos(angle))
@staticmethod
def setTheta(pose, angle):
orientation = quaternion_from_euler(0, 0, angle)
pose.orientation.x = orientation[0]
pose.orientation.y = orientation[1]
pose.orientation.z = orientation[2]
pose.orientation.w = orientation[3]
def computeBestPose(self):
# Setting parameters
theta_left = 60 * pi / 180 # angle opening on the left (POSITIVE VALUE between 0 and pi)
theta_right = 90 * pi / 180 # angle opening on the right (POSITIVE VALUE between 0 and pi)
distance_robot_vacuum_cleaner = 0.4 # in meter, on x axis only
length_vacuum_cleaner_arm = 0.55 # in meter
max_robot_dirt_distance = distance_robot_vacuum_cleaner + length_vacuum_cleaner_arm # when the robot looks into the dirt
# Using Al-Kashi theorem (in any triangle with usual notations
# (a, b, c for distances and alpha, beta, gamma for oposit angles):
# a^2 = b^2 + c^2 - 2b.c.cos(alpha)
# (in our case we use b = sqrt(a^2 - c^2 + 2b.c.cos(alpha)))
robot_dirt_distance_left = sqrt(length_vacuum_cleaner_arm**2 -
distance_robot_vacuum_cleaner**2 +
2 * length_vacuum_cleaner_arm *
distance_robot_vacuum_cleaner *
cos(theta_left))
robot_dirt_distance_right = sqrt(length_vacuum_cleaner_arm**2 -
distance_robot_vacuum_cleaner**2 +
2 * length_vacuum_cleaner_arm *
distance_robot_vacuum_cleaner *
cos(theta_right))
min_robot_dirt_distance = min(robot_dirt_distance_left,
robot_dirt_distance_right)
# For every robot position with a distance to the dirt between min_robot_dirt_distance and max_robot_dirt_distance
# there exits at least one orientation to clean the dirt
(robot_position, robot_orientation,
robot_rotation) = getCurrentRobotPosition()
angle_robot = self.normalize(robot_rotation[0]) # between -pi and pi
(x_robot, y_robot) = (robot_position[0], robot_position[1])
dx = self.dirt_position_.x - x_robot
dy = self.dirt_position_.y - y_robot
current_robot_dirt_distance = sqrt(dx**2 + dy**2)
angle_robot_dirt = atan2(dy, dx) # between -pi and +pi
(x_vacuum,
y_vacuum) = (x_robot +
distance_robot_vacuum_cleaner * cos(angle_robot),
y_robot +
distance_robot_vacuum_cleaner * sin(angle_robot))
dx = -x_vacuum + self.dirt_position_.x
dy = -y_vacuum + self.dirt_position_.y
angle_vacuum_dirt = atan2(dy, dx) # between -pi and +pi
diff_angle = self.normalize(angle_vacuum_dirt - angle_robot)
best_pose = Pose()
best_pose.position.x = x_robot
best_pose.position.y = y_robot
best_pose.position.z = 0
self.setTheta(best_pose, angle_robot)
print(-theta_right, diff_angle, theta_left)
print(min_robot_dirt_distance, current_robot_dirt_distance,
max_robot_dirt_distance)
if min_robot_dirt_distance <= current_robot_dirt_distance and current_robot_dirt_distance <= max_robot_dirt_distance and -theta_right <= diff_angle and diff_angle <= theta_left:
best_robot_angle = angle_robot
self.setTheta(best_pose, best_robot_angle)
#elif diff_angle <= 0: # WARNING - ??
# best_robot_angle = angle_robot_dirt + theta_right
# print('diff <= 0', best_robot_angle)
#elif diff_angle > 0:
# best_robot_angle = angle_robot_dirt - theta_left
# print('diff >= 0', best_robot_angle)
else: # first position in -angle_robot_dirt direction
#best_pose.position.x = self.dirt_position_.x + max_robot_dirt_distance*cos(-angle_robot_dirt) # pi/2
#best_pose.position.y = self.dirt_position_.y + max_robot_dirt_distance*sin(-angle_robot_dirt)
#self.setTheta(best_pose, angle_robot_dirt)
#print('positon', best_pose.position.x, best_pose.position.y)
# Help the names are not correct
accessible_poses = self.computeAccessiblePosesForVacuumCleaner()
best_pose = self.computeBestPoseForRobot(accessible_poses)
return best_pose
def removeDirt(self):
clean_pattern = rospy.ServiceProxy(self.clean_pattern_str_,
CleanPattern)
(robot_position, robot_orientation,
robot_rotation) = getCurrentRobotPosition()
theta = robot_rotation[0]
(x_robot, y_robot) = (robot_position[0], robot_position[1])
distance_robot_vacuum = 0.4
(x_vacuum, y_vacuum) = (x_robot + distance_robot_vacuum * cos(theta),
y_robot + distance_robot_vacuum * sin(theta)
) # WARNING DISTANCE HARDCODED
dx = -x_vacuum + self.dirt_position_.x
dy = -y_vacuum + self.dirt_position_.y
angle_vacuum_dirt = atan2(dy, dx) - theta # between -pi and +pi
print('vacuum angle', angle_vacuum_dirt * 180 / pi, dx, dy)
if self.use_vacuum_cleaner_:
request = CleanPatternRequest()
request.clean_pattern_params.directions = [
angle_vacuum_dirt * 180 / pi
]
request.clean_pattern_params.repetitions = [1]
request.clean_pattern_params.retract = True
response = clean_pattern(request)
return response.success
return True
def checkoutDirt(self):
# todo (rmb-ma)
rospy.sleep(1.)
# Implemented Behavior
def executeCustomBehavior(self):
assert (self.dirt_position_ is not None)
self.printMsg("Removing dirt located on ({}, {})".format(
self.dirt_position_.x, self.dirt_position_.y))
# The algorithm computes all the accessible poses for the vacuum cleaner
# (function computeAccessiblePosesForVacuumCleaner)
# around the dirt (radius hardcoded = the size of the arm)
# Then the algorithm find the best poses amoung all of them for the robot_
self.printMsg("> Computing the accessible poses")
#accessible_poses2d = self.computeAccessiblePosesForVacuumCleaner()
# if len(accessible_poses2d) == 0:
# # todo - handler (rmb-ma)
# self.printMsg("No accessible poses found.")
# return
# if self.handleInterrupt() >= 1:
# return
self.printMsg("> Computing the best accessible pose.")
not_moved = True
while not_moved:
best_pose3d = self.computeBestPose(
) #ForRobot()#accessible_poses2d)
if self.handleInterrupt() >= 1:
return
self.printMsg("> Moving to an accessible pose")
self.printMsg(" Position ({}, {})".format(best_pose3d.position.x,
best_pose3d.position.y))
self.move_base_handler_.setParameters(
goal_position=best_pose3d.position,
goal_orientation=best_pose3d.orientation,
header_frame_id='map',
goal_angle_tolerance=0.2,
goal_position_tolerance=0.10)
self.move_base_handler_.executeBehavior()
if self.handleInterrupt() >= 1:
return
if self.move_base_handler_.failed():
self.printMsg('Pose not accessible')
# remove the current position from the accessible ones
return
continue
else:
not_moved = False
self.printMsg("> Todo. Remove the dirt")
self.removeDirt()
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Checkout the dirt")
self.checkoutDirt()
class TrashcanEmptyingBehavior(behavior_container.BehaviorContainer):
# ========================================================================
# Description:
# Class which contains the behavior of trashcan emptying
# > Go to the trashcan
# > Take the trashcan
# > Go to the trolley
# > Empty the trashcan
# > Go to the trashcan location
# > Leave the trashcan
# ========================================================================
def __init__(self, behavior_name, interrupt_var, move_base_service_str):
super(TrashcanEmptyingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
(self.trolley_position_, self.trashcan_position_) = (None, None)
# Method for setting parameters for the behavior
def setParameters(self, trashcan_position, trolley_position):
self.trashcan_position_ = trashcan_position
self.trolley_position_ = trolley_position
# Method for returning to the standard pose of the robot
def returnToRobotStandardState(self):
# save current data if necessary
# undo or check whether everything has been undone
pass
def moveToGoal(self, goal):
self.move_base_handler_.setParameters(goal_position=goal,
goal_orientation=Quaternion(
x=0., y=0., z=0., w=1.),
header_frame_id='base_link')
self.move_base_handler_.executeBehavior()
def takeTrashcan(self):
# todo (rmb-ma)
rospy.sleep(2.)
def emptyTrashcan(self):
# todo (rmb-ma)
rospy.sleep(2)
def leaveTrashcan(self):
# todo (rmb-ma)
rospy.sleep(2)
# Implemented Behavior
def executeCustomBehavior(self):
assert (self.trashcan_position_ is not None
and self.trolley_position_ is not None)
self.printMsg("Executing trashcan behavior located on ({}, {})".format(
self.trashcan_position_.x, self.trashcan_position_.y))
# todo (rmb-ma): see how we can go there + see the locations to clean it
self.printMsg("> Moving to the trashcan")
self.moveToGoal(self.trashcan_position_)
if self.move_base_handler_.failed():
self.printMsg(
'Trashcan is not accessible. Failed to for emptying trashcan ({}, {})'
.format(self.trashcan_position_.x, self.trashcan_position_.y))
self.state_ = 4
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Take the trashcan")
self.takeTrashcan()
if self.handleInterrupt() >= 1:
return
self.printMsg("> Moving to the trolley located on ({}, {})".format(
self.trolley_position_.x, self.trolley_position_.y))
self.moveToGoal(self.trolley_position_)
if self.move_base_handler_.failed():
self.printMsg(
'Trolley is not accessible. Failed to for emptying trashcan ({}, {})'
.format(self.trashcan_position_.x, self.trashcan_position_.y))
self.state_ = 4
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Empty the trashcan")
self.emptyTrashcan()
if self.handleInterrupt() >= 1:
return
self.printMsg("> Going to the trashcan location")
self.moveToGoal(self.trashcan_position_)
if self.handleInterrupt() >= 1:
return
if self.move_base_handler_.failed():
self.printMsg(
'Trashcan location is not accessible. Failed to for emptying trashcan ({}, {})'
.format(self.trashcan_position_.x, self.trashcan_position_.y))
self.state_ = 4
return
self.printMsg("> Todo. Leave the trashcan")
self.leaveTrashcan()
if self.handleInterrupt() >= 1:
return
class TrashcanEmptyingBehavior(behavior_container.BehaviorContainer):
# ========================================================================
# Description:
# Class which contains the behavior of trashcan emptying
# > Go to the trashcan
# > Take the trashcan
# > Go to the trolley
# > Empty the trashcan
# > Go to the trashcan location
# > Leave the trashcan
# ========================================================================
def __init__(self, behavior_name, interrupt_var, move_base_service_str):
super(TrashcanEmptyingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
(self.trolley_pose_, self.trashcan_pose_) = (None, None)
(self.trolley_boundingbox_, self.trashcan_boundingbox_) = (None, None)
self.catch_trashcan_service_str_ = srv.CATCH_TRASHCAN_SERVICE_STR
self.empty_trashcan_service_str_ = srv.EMPTY_TRASHCAN_SERVICE_STR
self.leave_trashcan_service_str_ = srv.LEAVE_TRASHCAN_SERVICE_STR
self.transport_position_service_str_ = srv.TRANSPORT_POSITION_STR
self.rest_position_service_str_ = srv.REST_POSITION_STR
self.map_accessibility_service_str_ = srv.MAP_ACCESSIBILITY_SERVICE_STR
# Method for setting parameters for the behavior
def setParameters(self, trashcan_pose, trashcan_boundingbox, trolley_pose,
trolley_boundingbox):
self.trashcan_pose_ = trashcan_pose
self.trashcan_boundingbox_ = trashcan_boundingbox
self.trolley_pose_ = trolley_pose
self.trolley_boundingbox_ = trolley_boundingbox
def moveToGoalPosition(self, goal):
self.move_base_handler_.setParameters(
goal_position=goal.position, goal_orientation=goal.orientation)
self.move_base_handler_.executeBehavior()
def executeAction(self, action_name, target_pose=None):
goal = MoveToGoal()
if target_pose is not None:
goal = MoveToGoal()
goal.target_pos.pose = target_pose
goal.target_pos.header.frame_id = 'map'
client = actionlib.SimpleActionClient(action_name, MoveToAction)
client.wait_for_server()
client.send_goal(goal)
client.wait_for_result()
return client.get_result()
@withEnvironnment
def emptyTrashcan(self):
target_pose = Pose()
target_pose.position = self.trolley_pose_.position
target_pose.position.z = 1.2
(robot_position, _, _) = getCurrentRobotPosition()
delta_x = robot_position[0] - self.trolley_pose_.position.x
delta_y = robot_position[1] - self.trolley_pose_.position.y
yaw = acos(delta_x / sqrt(delta_x**2 + delta_y**2))
if delta_y < 0:
yaw *= -1
orientation = quaternion_from_euler(0, 0, yaw)
target_pose.orientation.x = orientation[0]
target_pose.orientation.y = orientation[1]
target_pose.orientation.z = orientation[2]
target_pose.orientation.w = orientation[3]
return self.executeAction(self.empty_trashcan_service_str_,
target_pose)
@withEnvironnment
def leaveTrashcan(self):
return self.executeAction(self.leave_trashcan_service_str_,
self.trashcan_pose_)
@withEnvironnment
def transportPosition(self):
return self.executeAction(self.transport_position_service_str_)
@withEnvironnment
def restPosition(self):
return self.executeAction(self.rest_position_service_str_)
@withEnvironnment
def catchTrashcan(self):
return self.executeAction(self.catch_trashcan_service_str_,
self.trashcan_pose_)
def computeAccessiblePosesAround(self, position, radius):
accessibility_checker = rospy.ServiceProxy(
self.map_accessibility_service_str_, CheckPerimeterAccessibility)
request = CheckPerimeterAccessibilityRequest()
center = Pose2D()
(center.x, center.y) = (position.x, position.y)
center.theta = 0
request.center = center
request.radius = 1. # todo (rmb-ma) use the value from the robotic arm
request.rotational_sampling_step = 0.3
response = accessibility_checker(request)
accessible_poses = response.accessible_poses_on_perimeter
return accessible_poses
def armCanAccessTrashcan(self, pose2d, orientation):
robot_pose_theta = pose2d.theta
orientation = self.trashcan_pose_.orientation
euler_angles = euler_from_quaternion(
[orientation.x, orientation.y, orientation.z, orientation.w])
trashcan_pose_theta = euler_angles[2]
return cos(trashcan_pose_theta -
robot_pose_theta) > 0.85 # todo rmb-ma fit the parameter
def computeRobotGoalPose(self, position, orientation=None, radius=1.):
position_in_map = Point()
position_in_map.x = position.x
position_in_map.y = position.y
position_in_map.z = 0
accessible_poses = self.computeAccessiblePosesAround(
position_in_map, radius)
if orientation is not None:
accessible_poses = list(
filter(
lambda pose: self.armCanAccessTrashcan(pose, orientation),
accessible_poses))
if len(accessible_poses) == 0:
return None # todo rmb-ma handle this case
accessible_pose = Pose()
accessible_pose.position.x = accessible_poses[0].x
accessible_pose.position.y = accessible_poses[0].y
accessible_pose.position.z = 0.
orientation = quaternion_from_euler(0, 0, accessible_poses[0].theta)
accessible_pose.orientation.x = orientation[0]
accessible_pose.orientation.y = orientation[1]
accessible_pose.orientation.z = orientation[2]
accessible_pose.orientation.w = orientation[3]
return accessible_pose
def setCollisionObject(self,
service_name,
pose,
bounding_box_lwh,
label,
id='0'):
collision_object = CollisionBox()
collision_object.object_id = label + id
collision_object.label = label
collision_object.id = id
collision_object.pose = pose
collision_object.bounding_box_lwh = bounding_box_lwh
# make request for loading environment
request = AddCollisionObjectRequest()
request.loading_method = 'primitive' # mesh
request.collision_objects.append(collision_object)
rospy.wait_for_service(service_name)
client = rospy.ServiceProxy(service_name, AddCollisionObject)
if not client.call(request):
rospy.logerr(
'Error while adding collision object (label: {}, id: {})'.
format(label, id))
return
def setTrolley(self):
if self.trolley_pose_ is None or self.trolley_boundingbox_ is None:
return
# todo rmb-ma compute best testing trolley location
object_pose = PoseStamped()
object_pose.header.frame_id = 'map'
object_pose.pose.position = self.trolley_pose_.position
object_pose.pose.orientation = self.trolley_pose_.orientation
self.setCollisionObject(
service_name='/ipa_planning_scene_creator/add_collision_objects',
pose=object_pose,
bounding_box_lwh=self.trolley_boundingbox_,
label='trolley')
def setTrashcan(self):
if self.trashcan_pose_ is None or self.trashcan_boundingbox_ is None:
return
object_pose = PoseStamped()
object_pose.header.frame_id = 'map'
orientation = self.trashcan_pose_.orientation
theta = euler_from_quaternion(
(orientation.x, orientation.y, orientation.z, orientation.w))[2]
object_pose.pose.position.x = self.trashcan_pose_.position.x + cos(
theta) * self.trashcan_boundingbox_.x / 2.
object_pose.pose.position.y = self.trashcan_pose_.position.y + sin(
theta) * self.trashcan_boundingbox_.x / 2.
object_pose.pose.position.z = self.trashcan_boundingbox_.z / 2.
object_pose.pose.orientation = self.trashcan_pose_.orientation
self.setCollisionObject(
service_name='/ipa_planning_scene_creator/add_collision_objects',
pose=object_pose,
bounding_box_lwh=self.trashcan_boundingbox_,
label='trashcan')
def loadEnvironment(self):
self.setTrashcan() # todo rmb-ma depending on the step
self.setTrolley()
def unloadEnvironment(self):
service_name = '/ipa_planning_scene_creator/remove_all_collision_objects'
rospy.wait_for_service(service_name)
client = rospy.ServiceProxy(service_name, Trigger)
request = TriggerRequest()
return not client.call(request)
def returnToRobotStandardState(self):
pass
# Implemented Behavior
def executeCustomBehavior(self):
assert (self.trashcan_pose_ is not None
and self.trolley_pose_ is not None)
self.printMsg("Executing trashcan behavior located on ({}, {})".format(
self.trashcan_pose_.position.x, self.trashcan_pose_.position.y))
# todo (rmb-ma): see how we can go there + see the locations to clean it
print("> Computing robot goal position")
robot_pose_for_catching_trashcan = self.computeRobotGoalPose(
self.trashcan_pose_.position,
orientation=self.trashcan_pose_.orientation,
radius=1.5)
self.printMsg("> Moving to the trashcan")
self.moveToGoalPosition(robot_pose_for_catching_trashcan)
if self.move_base_handler_.failed():
position = robot_pose_for_catching_trashcan.position
self.printMsg(
'Trashcan is not accessible. Failed to for emptying trashcan ({}, {})'
.format(position.x, position.y))
self.state_ = 4
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Catch the trashcan")
if not self.catchTrashcan().arrived:
#raise RuntimeError('Error occured while catching the trashcan')
print('error error error while doing something!')
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Going to transport position")
self.transportPosition()
if self.handleInterrupt() >= 1:
return
self.printMsg("> Computing robot goal position for trolley")
robot_pose_for_emptying_trashcan = self.computeRobotGoalPose(
self.trolley_pose_.position, radius=1.5)
self.printMsg("> Moving to the trolley located on ({}, {})".format(
self.trolley_pose_.position.x, self.trolley_pose_.position.y))
self.moveToGoalPosition(robot_pose_for_emptying_trashcan)
if self.move_base_handler_.failed():
self.printMsg(
'Trolley is not accessible. Failed to for emptying trashcan ({}, {})'
.format(robot_pose_for_emptying_trashcan.x,
robot_pose_for_emptying_trashcan.y))
self.state_ = 4
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Empty the trashcan")
if not self.emptyTrashcan().arrived:
#raise RuntimeError('Error while emptying the trashcan')
print('error error error while doing something!')
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Going to transport position")
if not self.transportPosition().arrived:
#raise RuntimeError('Error while moving to the transport position')
print('error error error while doing something!')
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Going to the trashcan location")
if self.handleInterrupt() >= 1:
return
if self.move_base_handler_.failed():
self.printMsg(
'Trashcan location is not accessible. Failed to for emptying trashcan ({}, {})'
.format(self.trashcan_position_.position.x,
self.trashcan_position_.position.y))
self.state_ = 4
return
self.printMsg("> Todo. Leave the trashcan")
if not self.leaveTrashcan():
#raise RuntimeError('Error while leaving the trashcan')
print('error error error while doing something!')
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Going to rest position")
if not self.restPosition():
#raise RuntimeError('Error while moving to the rest position')
print('error error error while doing something!')
return
if self.handleInterrupt() >= 1:
return
from threading import Thread
import services_params as srv
from std_srvs.srv import Trigger
from cob_object_detection_msgs.msg import DetectionArray
import rospy
from utils import projectToCamera
from dirt_removing_behavior import DirtRemovingBehavior
from move_base_behavior import MoveBaseBehavior
from geometry_msgs.msg import Pose2D, Pose, Quaternion
from tf.transformations import quaternion_from_euler
import services_params as srv
if __name__ == '__main__':
rospy.init_node('test_bug', anonymous=True)
mover = MoveBaseBehavior("MoveBaseBehavior", [0],
srv.MOVE_BASE_SERVICE_STR)
best_pose3d = Pose()
best_pose3d.position.x = -5.61
best_pose3d.position.y = -5.84
best_pose3d.position.z = 0
theta = -0.93
orientation = quaternion_from_euler(0, 0, -0.85)
best_pose3d.orientation.x = orientation[0]
best_pose3d.orientation.y = orientation[1]
best_pose3d.orientation.z = orientation[2]
best_pose3d.orientation.w = orientation[3]
mover.setParameters(goal_position=best_pose3d.position,
class DirtRemovingBehavior(behavior_container.BehaviorContainer):
# ========================================================================
# Description:
# Class which contains the behavior for removing dirt spots
# > Go to the dirt location
# > Clean
# ========================================================================
def __init__(self, behavior_name, interrupt_var, move_base_service_str,
map_accessibility_service_str):
super(DirtRemovingBehavior, self).__init__(behavior_name,
interrupt_var)
self.move_base_handler_ = MoveBaseBehavior("MoveBaseBehavior",
self.interrupt_var_,
move_base_service_str)
self.dirt_position_ = None
self.map_accessibility_service_str_ = map_accessibility_service_str
# Method for setting parameters for the behavior
def setParameters(self, dirt_position):
self.dirt_position_ = dirt_position
# Method for returning to the standard pose of the robot
def returnToRobotStandardState(self):
# save current data if necessary
# undo or check whether everything has been undone
pass
def computeAccessiblePoses(self):
accessibility_checker = rospy.ServiceProxy(
self.map_accessibility_service_str_, CheckPerimeterAccessibility)
request = CheckPerimeterAccessibilityRequest()
center = Pose2D()
(center.x, center.y) = (self.dirt_position_.x, self.dirt_position_.y)
center.theta = 0
request.center = center
request.radius = 1 # todo (rmb-ma) use the value from the detector
request.rotational_sampling_step = 0.3
response = accessibility_checker(request)
accessible_poses = response.accessible_poses_on_perimeter
return accessible_poses
# todo (rmb-ma). find the best accessible location
@staticmethod
def computeBestPose(accessible_locations):
assert (len(accessible_locations) > 0)
best_pose2d = accessible_locations[0]
best_pose3d = Pose()
(best_pose3d.position.x, best_pose3d.position.y) = (best_pose2d.x,
best_pose2d.y)
best_pose3d.orientation.w = best_pose2d.theta
return best_pose3d
def removeDirt(self):
# todo (rmb-ma)
pass
def checkoutDirt(self):
# todo (rmb-ma)
pass
# Implemented Behavior
def executeCustomBehavior(self):
assert (self.dirt_position_ is not None)
self.printMsg("Removing dirt located on ({}, {})".format(
self.dirt_position_.x, self.dirt_position_.y))
self.printMsg("> Computing the accessible poses")
accessible_poses2d = self.computeAccessiblePoses()
if len(accessible_poses2d) == 0:
# todo - handler (rmb-ma)
self.printMsg("No accessible poses found.")
return
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Computing the best accessible pose.")
best_pose3d = self.computeBestPose(accessible_poses2d)
if self.handleInterrupt() >= 1:
return
self.printMsg("> Moving to an accessible pose")
self.move_base_handler_.setParameters(
goal_position=best_pose3d.position,
goal_orientation=best_pose3d.orientation,
header_frame_id='base_link')
self.move_base_handler_.executeBehavior()
if self.handleInterrupt() >= 1:
return
if self.move_base_handler_.failed():
self.printMsg(
'Room center is not accessible. Failed to for removing dirt ({}, {})'
.format(self.dirt_position_.x, self.dirt_position_.y))
self.state_ = 4
return
self.printMsg("> Todo. Remove the dirt")
self.removeDirt()
if self.handleInterrupt() >= 1:
return
self.printMsg("> Todo. Checkout the dirt")
self.checkoutDirt()