def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def __make_position_goal(self, eef_link, goal):
"""
Helper method to create a pose goal out of a posestamped.
:param eef_link: name of the eef link
:type: str
:param goal: eef goal position
:type: PoseStamped
:return:
:type: PositionConstraint
"""
position_tolerance = 0.00001
orientation_tolerance = 0.001
position_goal = PositionConstraint()
position_goal.header = goal.header
position_goal.link_name = eef_link
position_goal.target_point_offset = Vector3()
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.SPHERE
primitive.dimensions.append(position_tolerance)
position_goal.constraint_region.primitives.append(primitive)
p = Pose()
p.position.x = goal.pose.position.x
p.position.y = goal.pose.position.y
p.position.z = goal.pose.position.z
p.orientation.w = 1
position_goal.constraint_region.primitive_poses.append(p)
position_goal.weight = 1.0
orientation_goal = OrientationConstraint()
orientation_goal.header = goal.header
orientation_goal.link_name = eef_link
orientation_goal.absolute_x_axis_tolerance = orientation_tolerance
orientation_goal.absolute_y_axis_tolerance = orientation_tolerance
orientation_goal.absolute_z_axis_tolerance = orientation_tolerance
orientation_goal.orientation = goal.pose.orientation
orientation_goal.weight = 1.0
return (position_goal, orientation_goal)
def to_collision_object(self):
"""
:return: the map as a collision object
:type: CollisionObject
"""
co = CollisionObject()
co.header.frame_id = "/odom_combined"
co.id = "map"
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.BOX
primitive.dimensions.append(self.cell_size)
primitive.dimensions.append(self.cell_size)
primitive.dimensions.append(2)
for x in range(0, len(self.field)):
for y in range(0, len(self.field[x])):
if self.field[x][y].is_free() or self.field[x][y].is_object():
continue
if self.field[x][y].is_obstacle():
primitive.dimensions[
primitive.BOX_Z] = self.get_cell_by_index(
x, y).highest_z * 2
else:
primitive.dimensions[
primitive.
BOX_Z] = self.get_average_z_of_surrounded_obstacles(
x, y) * 2
primitive.dimensions[primitive.BOX_Z] += 0.02
co.primitives.append(deepcopy(primitive))
pose = Pose()
(pose.position.x,
pose.position.y) = self.index_to_coordinates(x, y)
pose.orientation.w = 1
co.primitive_poses.append(pose)
return co
def matrixToPose(matrix):
t = tuple(translation_from_matrix(matrix))
q = tuple(quaternion_from_matrix(matrix))
return Pose(Point(t[0], t[1], t[2]), Quaternion(q[0], q[1], q[2], q[3]))
def __init__(self):
rospy.init_node('Store')
rospy.on_shutdown(self.shutdown)
rospy.loginfo('------------start test----------------')
self.test_bool = False
"""[State]
[Name]kinect_rec图像识别(识别所有物体)
"""
self.kinect_rec = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['object_list'])
with self.kinect_rec:
self.kinect_rec.userdata.rec = 25.0
StateMachine.add('RUNNODE',
RunNode_obj(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'GETLIST',
'error': 'error'
})
StateMachine.add('GETLIST',
GetObject_list(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
},
remapping={'object_list': 'object_list'})
"""[State]
[Name]sm_Pick
[function]识别某个物体调整位姿并抓取
"""
self.sm_Pick = StateMachine(outcomes=['succeeded', 'aborted', 'error'],
input_keys=['current_obj'])
with self.sm_Pick:
self.sm_Pick.userdata.target_mode = 0
self.sm_Pick.userdata.objmode = -1
self.sm_Pick.userdata.rec = 1.0
StateMachine.add('RUNNODE_IMG',
RunNode_img(),
transitions={
'succeeded': 'WAIT',
'aborted': 'aborted'
})
StateMachine.add('WAIT',
Wait(),
transitions={
'succeeded': 'FIND_OBJECT',
'error': 'error'
})
self.sm_Pick.userdata.object_pos = PointStamped()
self.sm_Pick.userdata.objmode = -1
StateMachine.add('FIND_OBJECT',
FindObject(),
transitions={
'succeeded': 'POS_JUSTFY',
'aborted': 'FIND_OBJECT',
'error': 'SPEAK'
},
remapping={
'name': 'current_obj',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
#making the xm foreward the object may make the grasping task easier
self.sm_Pick.userdata.pose = Pose()
StateMachine.add('POS_JUSTFY',
PosJustfy(),
remapping={
'object_pos': 'object_pos',
'pose': 'pose'
},
transitions={
'succeeded': 'NAV_TO',
'aborted': 'aborted',
'error': 'error'
})
StateMachine.add('NAV_TO',
NavStack(),
transitions={
'succeeded': 'RUNNODE_IMG2',
'aborted': 'NAV_TO',
'error': 'error'
},
remapping={"pos_xm": 'pose'})
StateMachine.add('RUNNODE_IMG2',
RunNode_img(),
transitions={
'succeeded': 'FIND_AGAIN',
'aborted': 'aborted'
})
StateMachine.add('FIND_AGAIN',
FindObject(),
transitions={
'succeeded': 'PICK',
'aborted': 'FIND_AGAIN',
'error': 'SPEAK'
},
remapping={
'name': 'current_obj',
'object_pos': 'object_pos',
'objmode': 'objmode'
})
self.sm_Pick.userdata.arm_mode_1 = 3
StateMachine.add('PICK',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
},
remapping={
'arm_ps': 'object_pos',
'mode': 'arm_mode_1'
})
self.sm_Pick.userdata.sentences = 'xiao meng can not find the thing'
StateMachine.add('SPEAK',
Speak(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
self.xm_Place = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
output_keys=['position_list'],
input_keys=['current_obj', 'position_list'])
with self.xm_Place:
self.xm_Place.userdata.mode = 2
self.xm_Place.userdata.objmode = 2
StateMachine.add('GET_POSITION',
Get_position(),
transitions={
'succeeded': 'PLACE',
'error': 'error'
},
remapping={'arm_ps': 'arm_ps'})
StateMachine.add('PLACE',
ArmCmd(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
"""[State]
[Name]grasp_from_list
[function]控制状态机，从列表里依次提取是sm_Pick和sm_Place的上层状态机
"""
self.grasp_from_list = StateMachine(
outcomes=['succeeded', 'aborted', 'error'],
input_keys=['object_list'])
with self.grasp_from_list:
self.grasp_from_list.userdata.current_task = 0
self.grasp_from_list.userdata.degree = 3.1415
self.grasp_from_list.userdata.pos_xm_place = gpsr_target['place'][
'pos']
self.grasp_from_list.userdata.pos_xm_pick = gpsr_target['pick'][
'pos']
self.grasp_from_list.userdata.pos_xm_place = Pose(
Point(1.65, 1.34, 0), Quaternion(0, 0, 0, 1))
self.grasp_from_list.userdata.position_list = {
1:
PointStamped(Header(frame_id='base_link'),
Point(-0.300, 0.750, 0.670)),
2:
PointStamped(Header(frame_id='base_link'),
Point(-0.300, 0.750, 0.355)),
3:
PointStamped(Header(frame_id='base_link'),
Point(-0.300, 0.750, 0.040)),
4:
PointStamped(Header(frame_id='base_link'),
Point(-0.300, 0.750, 0.000))
}
StateMachine.add('GETOBJ',
Get_object(),
transitions={
'succeeded': 'GRASP',
'finish': 'succeeded',
'error': 'error'
},
remapping={'current_obj': 'current_obj'})
StateMachine.add('GRASP',
self.sm_Pick,
transitions={
'succeeded': 'TURN',
'aborted': 'TURN',
'error': 'error'
})
StateMachine.add('TURN',
TurnDegree(),
transitions={
'succeeded': 'MOVE_PLACE',
'aborted': 'MOVE_PLACE',
'error': 'error'
})
# StateMachine.add('MOVE_PLACE',
# NavStack(),
# transitions={'succeeded':'PLACE','aborted':'MOVE_PLACE','error':'error'},
# remapping = {'pos_xm':'pos_xm_place'})
StateMachine.add('PLACE',
ArmCmdPlace(),
transitions={
'succeeded': 'TURN2',
'aborted': 'TURN2'
},
remapping={'Object_Num': 'current_task'})
StateMachine.add('MOVE_PLACE',
NavStack(),
transitions={
'succeeded': 'PLACE',
'aborted': 'MOVE_PLACE',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm_place'})
StateMachine.add('TURN2',
TurnDegree(),
transitions={
'succeeded': 'MOVE_PICK2',
'aborted': 'MOVE_PICK2',
'error': 'error'
})
StateMachine.add('MOVE_PICK2',
NavStack(),
transitions={
'succeeded': 'GETOBJ',
'aborted': 'MOVE_PICK2',
'error': 'error'
},
remapping={'pos_xm': 'pos_xm_pick'})
self.store = StateMachine(outcomes=['succeeded', 'aborted', 'error'])
with self.store:
self.store.userdata.rec = 3.0
self.store.userdata.degree = 3.1416
self.store.userdata.pos_xm = gpsr_target['pick']['pos']
StateMachine.add('TURN',
TurnDegree(),
transitions={
'succeeded': 'RECO',
'aborted': 'aborted',
'error': 'error'
})
# StateMachine.add('NAV',
# NavStack(),
# transitions = {'succeeded':'RECO','aborted':'NAV','error':'error'})
StateMachine.add('RECO',
self.kinect_rec,
transitions={
'succeeded': 'GRASP_FROM_LIST',
'aborted': 'aborted',
'error': 'error'
},
remapping={'object_list': 'object_list'})
# StateMachine.add('WAIT',
# Wait(),
# transitions = {'succeeded':'GRASP_FROM_LIST','error':'error'})
StateMachine.add('GRASP_FROM_LIST',
self.grasp_from_list,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'error': 'error'
})
try:
out = self.store.execute()
except Exception, e:
rospy.logerr('test failed , loser loser, don\'t have dinner')
rospy.logerr(e)
#encoding:utf8
"""
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
gpsr_target = {
'speaker': {
'pos':
Pose(Point(2.411, 0.221, 0.000), Quaternion(0.000, 0.000, 0.016,
1.000)),
'mode':
1
},
'end': {
'pos':
Pose(Point(7.829, 3.997, 0.000), Quaternion(0.000, 0.000, 0.000,
1.000)),
'mode':
1
},
'Gray': {
'pos': Pose(),
'mode': 1
},
'David': {
#encoding:utf8
"""
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
target={
'speaker': {'pos': Pose(Point(1.358, -0.241, 0.000),Quaternion(0.000, 0.000, 0.068, 0.997)), 'mode': 1 },
'Gray': {'pos': Pose(), 'mode': 1 },
'David': {'pos': Pose(), 'mode': 1 },
'Daniel': {'pos': Pose(), 'mode': 1 },
'Jack': {'pos': Pose(), 'mode': 1 },
'Jenny': {'pos': Pose(), 'mode': 1 },
'Michael': {'pos': Pose(), 'mode': 1 },
'Lucy': {'pos': Pose(), 'mode': 1 },
'Peter': {'pos': Pose(), 'mode': 1 },
'Tom': {'pos': Pose(), 'mode': 1 },
'Jordan': {'pos': Pose(), 'mode': 1 },
'kitchen': {'pos': Pose(Point(4.881, 4.595, 0.000),Quaternion(0.000, 0.000, 0.730, 0.683)), 'mode': 1 },
'livingroom': {'pos': Pose(Point(2.336, -1.790, 0.000),Quaternion(0.000, 0.000, 0.005, 1.000)), 'mode': 1 },##########gai
'bedroom': {'pos': Pose(Point(0.000, 0.000, 0.000),Quaternion(0.000, 0.000, 0.006, 1.000)), 'mode': 1 },########gai
'init_pose':{'pos': Pose(Point(9.122, 1.874, 0.000),Quaternion(0.000, 0.000, -0.048, 0.999)), 'mode': 1 },
'dining-room': {'pos': Pose(Point(6.927, 5.776, 0.000),Quaternion(0.000, 0.000, 0.053, 0.999)), 'mode': 1 },
#encoding:utf8
"""
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
gpsr_target = {
'speaker': {
'pos':
Pose(Point(1.984, 0.187, 0.000), Quaternion(0.000, 0.000, -0.035,
0.999)),
'mode':
1
},
'end': {
'pos':
Pose(Point(7.829, 3.997, 0.000), Quaternion(0.000, 0.000, 0.000,
1.000)),
'mode':
1
},
'Gray': {
'pos': Pose(),
'mode': 1
},
'David': {
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
target = {
0x09: {
'name':
'SPEAKER',
'pos':
Pose(Point(4.817, 1.928, 0.000), Quaternion(0.000, 0.000, 0.016,
1.000)),
'mode':
1
},
0x10: {
'name': 'Gray',
'pos': Pose(),
'mode': 1
},
0x11: {
'name': 'David',
'pos': Pose(),
'mode': 1
},
0x12: {
'name': 'Daniel',
#encoding:utf8
"""
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
gpsr_target = {
'speaker': {
'pos':
Pose(Point(3.689, 1.117, 0.000), Quaternion(0.000, 0.000, -0.093, 1)),
'mode':
1
},
#'speaker':{'pos':Pose(Point(7.408, -4.733 ,0.000), Quaternion(0.000, 0.000, 0.0403 , 1)),'mode': 1},
'end': {
'pos':
Pose(Point(11.363, 8.234, 0.000), Quaternion(0.000, 0.000, 0.619,
0.780)),
'mode':
1
},
'Gray': {
'pos': Pose(),
'mode': 1
},
#encoding:utf8
"""
it is a stuff list for GPSR
it contains the pose of all the stuff
"""
from geometry_msgs.msg import *
from geometry_msgs.msg._Pose import Pose
# mode = 1 在桌子上
# mode = 2 在架子上
gpsr_target={
'speaker': {'pos': Pose(Point(6.089 , 2.031, 0.000), Quaternion(0.000, 0.000, -0.688, 0.726)), 'mode': 1 },
'end': {'pos': Pose(Point(7.829, 3.997, 0.000),Quaternion(0.000, 0.000, 0.000, 1.000)), 'mode': 1 },
'Gray': {'pos': Pose(), 'mode': 1 },
'David': {'pos': Pose(), 'mode': 1 },
'Daniel': {'pos': Pose(), 'mode': 1 },
'Jack': {'pos': Pose(), 'mode': 1 },
'Jenny': {'pos': Pose(), 'mode': 1 },
'Michael': {'pos': Pose(), 'mode': 1 },
'Lucy': {'pos': Pose(), 'mode': 1 },
'Peter': {'pos': Pose(), 'mode': 1 },
'Tom': {'pos': Pose(), 'mode': 1 },
'Jordan': {'pos': Pose(), 'mode': 1 },
'kitchen_table_1': {'pos': Pose(Point(2.390, -6.656, 0.000),Quaternion(0.000, 0.000, 0.999, -0.036)), 'mode': 1 },
'kitchen_table_2': {'pos': Pose(Point(2.390, -6.656, 0.000),Quaternion(0.000, 0.000, 0.999, -0.036)), 'mode': 1 },
#################### winter_test ################################