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