def talker(self):
rate = rospy.Rate(self.rate_) # 0.2Hz
while not rospy.is_shutdown():
self.mutex_.acquire()
if not self.is_running_:
self.mutex_.release()
return
self.mutex_.release()
rate.sleep()
if random.random() > 0.1:
continue
detection = Detection()
(translation, _, _) = getCurrentRobotPosition()
(x, y) = (translation[0],
translation[1]) if translation is not None else (0, 0)
detection.pose.pose.position.x = round(
x + random.random() * 0.02 - 0.01, 0)
detection.pose.pose.position.y = round(
y + random.random() * 0.02 - 0.01, 0)
detections = DetectionArray()
detections.detections = [detection]
self.publisher_.publish(detections)
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 executeCustomBehavior(self):
room_sequence_goal = FindRoomSequenceWithCheckpointsGoal()
room_sequence_goal.input_map = self.database_.global_map_data_.map_image_
room_sequence_goal.map_resolution = self.database_.global_map_data_.map_resolution_
room_sequence_goal.map_origin = self.database_.global_map_data_.map_origin_
room_sequence_goal.robot_radius = self.robot_radius_
room_sequence_goal.room_information_in_pixel = self.room_information_in_pixel_
(robot_pose_translation, robot_pose_rotation,
robot_pose_rotation_euler) = getCurrentRobotPosition()
if robot_pose_translation is not None:
self.printMsg("Robot starting from position ({}, {})".format(
*robot_pose_translation))
room_sequence_goal.robot_start_coordinate.position = Point32(
x=robot_pose_translation[0], y=robot_pose_translation[1])
else:
self.printMsg(
"Warning: tf lookup failed, taking (0,0) as robot_start_coordinate."
)
room_sequence_goal.robot_start_coordinate.position = Point32(x=0,
y=0)
room_sequence_goal.robot_start_coordinate.orientation = Quaternion(
x=0., y=0., z=0., w=0.) # todo: normalized quaternion
room_sequence_client = actionlib.SimpleActionClient(
str(self.service_str_), FindRoomSequenceWithCheckpointsAction)
self.printMsg("Running sequencing action...")
self.room_sequence_result_ = self.runAction(
room_sequence_client, room_sequence_goal)['result']
self.printMsg("Room sequencing completed.")
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
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)
def computeCoveragePath(self, room_id):
self.printMsg(
'Starting computing coverage path of room ID {}'.format(room_id))
room_explorer = room_exploration_behavior.RoomExplorationBehavior(
"RoomExplorationBehavior", self.interrupt_var_,
self.room_exploration_service_str_)
room_center = self.room_information_in_meter_[room_id].room_center
room_map_data = self.database_handler_.database_.getRoomById(
room_id).room_map_data_
(robot_position, _, _) = getCurrentRobotPosition()
starting_position = (
robot_position[0],
robot_position[1]) if robot_position is not None else (
room_center.x, room_center.y)
room_explorer.setParameters(
input_map=room_map_data,
map_resolution=self.database_handler_.database_.global_map_data_.
map_resolution_,
map_origin=self.database_handler_.database_.global_map_data_.
map_origin_,
robot_radius=self.robot_radius_,
coverage_radius=self.coverage_radius_,
field_of_view=self.
field_of_view_, # this field of view represents the off-center iMop floor wiping device
field_of_view_origin=self.field_of_view_origin_,
starting_position=Pose2D(x=starting_position[0],
y=starting_position[1],
theta=0.),
# todo: determine theta
planning_mode=2)
room_explorer.executeBehavior()
self.printMsg('Coverage path of room ID {} computed.'.format(room_id))
return room_explorer.exploration_result_.coverage_path_pose_stamped
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 executeCustomBehavior(self):
self.move_base_handler_ = move_base_behavior.MoveBaseBehavior(
"MoveBaseBehavior", self.interrupt_var_,
self.move_base_service_str_)
self.room_explorer_ = room_exploration_behavior.RoomExplorationBehavior(
"RoomExplorationBehavior", self.interrupt_var_,
self.room_exploration_service_str_)
self.path_follower_ = move_base_path_behavior.MoveBasePathBehavior(
"MoveBasePathBehavior_PathFollowing", self.interrupt_var_,
self.move_base_path_service_str_)
self.wall_follower_ = move_base_wall_follow_behavior.MoveBaseWallFollowBehavior(
"MoveBaseWallFollowBehavior", self.interrupt_var_,
self.move_base_wall_follow_service_str_)
for current_checkpoint_index in range(
len(self.sequence_data_.checkpoints)):
for current_room_index in self.sequence_data_.checkpoints[
current_checkpoint_index].room_indices:
self.printMsg(
"Attending to next room with current_room_index=" +
str(current_room_index))
# Interruption opportunity
if self.handleInterrupt() >= 1:
return
# Room exploration
"""
For room exploration:
map_data = self.map_data_
input_map = self.getMapSegmentAsImageMsg(current_room_index)
map_resolution = self.map_data_.map_resolution
map_origin = self.map_data_.map_origin
robot_radius = 0.325
coverage_radius = 0.25
field_of_view = [Point32(x=0.04035, y=0.136), Point32(x=0.04035, y=-0.364), Point32(x=0.54035, y=-0.364), Point32(x=0.54035, y=0.136)] # this field of view represents the off-center iMop floor wiping device
starting_position = Pose2D(x=1., y=0., theta=0.)
planning_mode = 2
"""
(robot_position, _, _) = getCurrentRobotPosition()
starting_position = (
robot_position[0],
robot_position[1]) if robot_position is not None else (
current_room_center.x, current_room_center.y)
current_room_center = self.segmentation_data_.room_information_in_meter[
current_room_index].room_center
current_room_map = self.getMapSegmentAsImageMsg(
self.opencv_segmented_map_, current_room_index)
self.room_explorer_.setParameters(
current_room_map,
self.map_data_.map_resolution,
self.map_data_.map_origin,
robot_radius=self.robot_radius_,
coverage_radius=self.coverage_radius_,
field_of_view=self.
field_of_view_, # this field of view represents the off-center iMop floor wiping device
field_of_view_origin=self.field_of_view_origin_,
starting_position=Pose2D(
x=starting_position[0],
y=starting_position[1],
theta=0.), # todo: determine current theta
planning_mode=2)
self.room_explorer_.executeBehavior()
if len(self.room_explorer_.exploration_result_.
coverage_path_pose_stamped) == 0:
continue
# Interruption opportunity
if self.handleInterrupt() >= 1:
return
#rospy.sleep(20)
#continue
# Robot movement into next room
"""
For movement to room:
goal_position = self.segmentation_data_.room_information_in_meter[current_room_index].room_center
goal_orientation = Quaternion(x=0., y=0., z=0., w=0.)
header_frame_id = 'base_link'
"""
# todo: hack: reactivate this code
"""
self.printMsg("Moving to room_center in meter=" + str(self.segmentation_data_.room_information_in_meter[current_room_index].room_center))
self.move_base_handler_.setParameters(
self.segmentation_data_.room_information_in_meter[current_room_index].room_center,
Quaternion(x=0., y=0., z=0., w=1.),
'base_link'
)
self.move_base_handler_.executeBehavior()
"""
# Interruption opportunity
if self.handleInterrupt() >= 1:
return
# baker_brush_cleaning_module_interface: turn on the cleaning device (service "start_brush_cleaner")
if self.use_cleaning_device_: # todo: hack: cleaning device can be turned off for trade fair show
self.printMsg("Start cleaning with " +
self.start_cleaning_service_str_)
rospy.wait_for_service(self.start_cleaning_service_str_)
try:
req = rospy.ServiceProxy(
self.start_cleaning_service_str_,
std_srvs.srv.Trigger)
resp = req()
print "Start cleaning returned with success status " + str(
resp.success)
except rospy.ServiceException, e:
print "Service call to " + self.start_cleaning_service_str_ + " failed: %s" % e
# coverage_monitor_server: set the robot configuration (robot_radius, coverage_radius, coverage_offset) with dynamic reconfigure
# and turn on logging of the cleaned path (service "start_coverage_monitoring")
try:
print "Calling dynamic reconfigure at the coverage_monitor_server to set robot radius, coverage_radius, and coverage offset and start coverage monitoring."
client = dynamic_reconfigure.client.Client(
self.coverage_monitor_dynamic_reconfigure_service_str_,
timeout=5)
rospy.wait_for_service(
self.coverage_monitor_dynamic_reconfigure_service_str_
+ "/set_parameters")
client.update_configuration({
"map_frame":
self.map_data_.map.header.frame_id,
"robot_frame":
self.robot_frame_id_,
"coverage_radius":
self.coverage_radius_,
"coverage_circle_offset_transform_x":
0.5 *
(self.field_of_view_[0].x + self.field_of_view_[2].x),
"coverage_circle_offset_transform_y":
0.5 *
(self.field_of_view_[0].y + self.field_of_view_[1].y),
"coverage_circle_offset_transform_z":
0.0,
"robot_trajectory_recording_active":
True
})
except rospy.ServiceException, e:
print "Dynamic reconfigure request to " + self.coverage_monitor_dynamic_reconfigure_service_str_ + " failed: %s" % e
# Explored path follow
"""
For path follow movement:
target_poses = exploration_result.coverage_path_pose_stamped
area_map = current_room_map
path_tolerance = 0.2
goal_position_tolerance = 0.5
goal_angle_tolerance = 1.57
"""
self.path_follower_.setParameters(
self.room_explorer_.exploration_result_.
coverage_path_pose_stamped, current_room_map, 0.2, 0.5,
1.57)
self.path_follower_.executeBehavior()
# Interruption opportunity
if self.handleInterrupt() >= 1:
return
# Wall follow
"""
For wall following movement:
target_poses = exploration_result.coverage_path_pose_stamped
path_tolerance = 0.2
goal_position_tolerance = 0.4
goal_angle_tolerance = 3.14
"""
# receive coverage map from coverage monitor
self.printMsg("Receive coverage image from coverage monitor " +
self.receive_coverage_image_service_str_)
rospy.wait_for_service(
self.receive_coverage_image_service_str_)
try:
req = rospy.ServiceProxy(
self.receive_coverage_image_service_str_,
ipa_building_msgs.srv.CheckCoverage)
req.input_map = current_room_map
req.map_resolution = self.map_data_.map_resolution
req.map_origin = self.map_data_.map_origin
req.field_of_view = self.field_of_view_
req.field_of_view_origin = self.field_of_view_origin_
req.coverage_radius = self.coverage_radius_
req.check_for_footprint = False
req.check_number_of_coverages = False
self.coverage_map_response_ = req()
print "Receive coverage image returned with success status " + str(
resp.success)
except rospy.ServiceException, e:
print "Service call to " + self.receive_coverage_image_service_str_ + " failed: %s" % e