def test5_1(self):
'''
3 single unknowns
'''
m = Map(2)
self.assertTrue(m.get_percent_cleared() == 0.0, m.get_percent_cleared())
m = self.make_free_map()
self.assertTrue(m.get_percent_cleared() == 1.0, m.get_percent_cleared())
m.get_cell_by_index(3,4).set_unknown()
self.assertTrue(m.get_percent_cleared() < 1.0, m.get_percent_cleared())
def _handle_scan_map(self, req):
taskdata = req.taskdata
rospy.loginfo('Scanning map')
utils.map = Map(2)
self._scan_map()
return TaskDataServiceResponse(taskdata=taskdata,
result=self.RETURN_VAL_MAP_SCANNED)
def _handle_add_point_cloud(self, req):
if utils.map is None:
utils.map = Map(2)
if req.arm_origin is not None:
print(req.arm_origin)
utils.map.add_point_cloud(req.arm_origin, scene_cam=req.scenecam)
else:
utils.map.add_point_cloud(scene_cam=req.scenecam)
return AddPointCloudResponse()
def test5_1(self):
'''
3 single unknowns
'''
m = Map(2)
self.assertTrue(m.get_percent_cleared() == 0.0,
m.get_percent_cleared())
m = self.make_free_map()
self.assertTrue(m.get_percent_cleared() == 1.0,
m.get_percent_cleared())
m.get_cell_by_index(3, 4).set_unknown()
self.assertTrue(m.get_percent_cleared() < 1.0, m.get_percent_cleared())
def execute(self, userdata):
rospy.loginfo('Executing state ScanMapMastCam')
if utils.manipulation is None:
utils.manipulation = Manipulation(userdata.yaml)
rospy.sleep(2)
utils.map = Map(2)
# arm_base = utils.manipulation.get_base_origin()
# rospy.sleep(4)
utils.manipulation.pan_tilt(0.2, 0.5)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0.2825, 0.775)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0, 1.1)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(-0.2825, 0.775)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(-0.2, 0.5)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0, 0.45)
if not userdata.enable_movement:
return 'mapScanned'
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# utils.manipulation.get_planning_scene().add_object(co)
rospy.sleep(2)
n = 8
for i in range(0, n):
a = 2 * pi * ((i + 0.0) / (n + 0.0))
goal_base_pose = PoseStamped()
goal_base_pose.header.frame_id = "/odom_combined"
goal_base_pose.pose.position = Point(cos(a), sin(a), 0)
if goal_base_pose.pose.position.x < 0 and goal_base_pose.pose.position.y < 0 or \
goal_base_pose.pose.position.x > 0 and goal_base_pose.pose.position.y > 0:
continue
goal_base_pose.pose.position = set_vector_length(
0.25, goal_base_pose.pose.position)
goal_base_pose.pose.orientation.w = 1
if utils.manipulation.move_base(goal_base_pose):
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(
arm_origin=utils.manipulation.get_base_origin().point,
scene_cam=True)
break
# utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)
# utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)
# utils.map.publish_as_marker()
if userdata.yaml.task_type == Task.TASK_5:
utils.map.all_unknowns_to_obstacle()
#utils.map.remove_puzzle_fixture(userdata.yaml)
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
return 'mapScanned'
def make_free_map(self):
m = Map(2)
for x in xrange(m.num_of_cells):
for y in xrange(m.num_of_cells):
m.get_cell_by_index(x,y).set_free()
return m
def make_free_map(self):
m = Map(2)
for x in xrange(m.num_of_cells):
for y in xrange(m.num_of_cells):
m.get_cell_by_index(x, y).set_free()
return m
