def main():
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=0)
rospy.sleep(2)
# centre table
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 1. # position of the table in the world frame
p1.pose.position.y = 0.
p1.pose.position.z = 0.
# left table (from baxter's perspective)
p1_l = PoseStamped()
p1_l.header.frame_id = robot.get_planning_frame()
p1_l.pose.position.x = 0.5 # position of the table in the world frame
p1_l.pose.position.y = 1.
p1_l.pose.position.z = 0.
p1_l.pose.orientation.x = 0.707
p1_l.pose.orientation.y = 0.707
# right table (from baxter's perspective)
p1_r = PoseStamped()
p1_r.header.frame_id = robot.get_planning_frame()
p1_r.pose.position.x = 0.5 # position of the table in the world frame
p1_r.pose.position.y = -1.
p1_r.pose.position.z = 0.
p1_r.pose.orientation.x = 0.707
p1_r.pose.orientation.y = 0.707
# open back shelf
p2 = PoseStamped()
p2.header.frame_id = robot.get_planning_frame()
p2.pose.position.x = 1.2 # position of the table in the world frame
p2.pose.position.y = 0.0
p2.pose.position.z = 0.75
p2.pose.orientation.x = 0.5
p2.pose.orientation.y = -0.5
p2.pose.orientation.z = -0.5
p2.pose.orientation.w = 0.5
pw = PoseStamped()
pw.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p_ob1 = PoseStamped()
p_ob1.header.frame_id = robot.get_planning_frame()
p_ob1.pose.position.x = .92
p_ob1.pose.position.y = 0.3
p_ob1.pose.position.z = 0.89
# the ole duck
p_ob2 = PoseStamped()
p_ob2.header.frame_id = robot.get_planning_frame()
p_ob2.pose.position.x = 0.87
p_ob2.pose.position.y = -0.4
p_ob2.pose.position.z = 0.24
# clean environment
scene.remove_world_object("table_center")
scene.remove_world_object("table_side_left")
scene.remove_world_object("table_side_right")
scene.remove_world_object("shelf")
scene.remove_world_object("wall")
scene.remove_world_object("part")
scene.remove_world_object("duck")
rospy.sleep(1)
scene.add_box("table_center", p1,
size=(.5, 1.5, 0.4)) # dimensions of the table
scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4))
scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4))
scene.add_mesh(
"shelf",
p2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/bookshelf_light.stl",
size=(.025, .01, .01))
scene.add_plane("wall", pw, normal=(0, 1, 0))
part_size = (0.07, 0.05, 0.12)
scene.add_box("part", p_ob1, size=part_size)
scene.add_mesh(
"duck",
p_ob2,
"/home/nikhildas/ros_ws/baxter_interfaces/baxter_moveit_config/baxter_scenes/duck.stl",
size=(.001, .001, .001))
rospy.sleep(1)
print(scene.get_known_object_names())
## ---> SET COLOURS
table_color = color_norm([105, 105,
105]) # normalize colors to range [0, 1]
shelf_color = color_norm([139, 69, 19])
duck_color = color_norm([255, 255, 0])
_colors = {}
setColor('table_center', _colors, table_color, 1)
setColor('table_side_left', _colors, table_color, 1)
setColor('table_side_right', _colors, table_color, 1)
setColor('shelf', _colors, shelf_color, 1)
setColor('duck', _colors, duck_color, 1)
sendColors(_colors, scene)
p.pose.position.x = 0
p.pose.position.y = 0.8
p.pose.position.z = 0.1
p.pose.orientation.w = 1.0
scene.add_sphere("ball", p, 0.1)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.1
scene.add_box("table", p, (1.0, 2.6, 0.2))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.4
scene.add_plane("ground_plane", p)
rospy.sleep(20)
## We can get the name of the reference frame for this robot
print ">>>>> Reference frame: %s" % group.get_planning_frame()
print ">>>>> Printing robot state"
print robot.get_current_state()
print ">>>>> Printing robot pose"
print group.get_current_pose()
## Planning to a Pose goal
print ">>>>> Generating plan"
pose_target = geometry_msgs.msg.Pose()
#pose_target.orientation.w = 1.0
def init():
global marker_array_pub, marker_pub, tf_broadcaster, tf_listener
global move_group, turntable
global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir
rospy.init_node('acquisition')
camera_mesh = rospy.get_param('~camera_mesh', None)
camera_orientation = rospy.get_param('~camera_orientation', None)
camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0])
camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1])
min_distance = rospy.get_param('~min_distance', 0.2)
max_distance = rospy.get_param('~max_distance', min_distance)
max_velocity = rospy.get_param('~max_velocity', 0.1)
num_positions = rospy.get_param('~num_positions', 15)
num_spins = rospy.get_param('~num_spins', 8)
object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2]))
photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0])
photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0])
ptpip = rospy.get_param('~ptpip', None)
reach = rospy.get_param('~reach', 0.85)
simulation = '/gazebo' in get_node_names()
test = rospy.get_param('~test', True)
turntable_pos = rospy.get_param(
'~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05])
turntable_radius = rospy.get_param('~turntable_radius', 0.2)
wall_thickness = rospy.get_param('~wall_thickness', 0.04)
marker_array_pub = rospy.Publisher('visualization_marker_array',
MarkerArray,
queue_size=1,
latch=True)
marker_pub = rospy.Publisher('visualization_marker',
Marker,
queue_size=1,
latch=True)
tf_broadcaster = tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
move_group = MoveGroupCommander('manipulator')
move_group.set_max_acceleration_scaling_factor(
1.0 if simulation else max_velocity)
move_group.set_max_velocity_scaling_factor(
1.0 if simulation else max_velocity)
robot = RobotCommander()
scene = PlanningSceneInterface(synchronous=True)
try:
st_control = load_source(
'st_control',
os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts',
'iai_scanning_table', 'st_control.py'))
turntable = st_control.ElmoUdp()
if turntable.check_device():
turntable.configure()
turntable.reset_encoder()
turntable.start_controller()
turntable.set_speed_deg(30)
except Exception as e:
print(e)
sys.exit('Could not connect to turntable.')
if simulation:
move_home()
elif not test:
working_dir = rospy.get_param('~working_dir', None)
if working_dir is None:
sys.exit('Working directory not specified.')
elif not camera.init(
os.path.join(os.path.dirname(os.path.abspath(__file__)), '..',
'out', working_dir, 'images'), ptpip):
sys.exit('Could not initialize camera.')
# add ground plane
ps = PoseStamped()
ps.header.frame_id = robot.get_planning_frame()
scene.add_plane('ground', ps)
# add photobox
ps.pose.position.x = photobox_pos[
0] + photobox_size[0] / 2 + wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_left', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[
0] - photobox_size[0] / 2 - wall_thickness / 2
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_right', ps,
(wall_thickness, photobox_size[1], photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[
1] - photobox_size[1] / 2 - wall_thickness / 2
ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2
scene.add_box('box_wall_back', ps,
(photobox_size[0], wall_thickness, photobox_size[2]))
ps.pose.position.x = photobox_pos[0]
ps.pose.position.y = photobox_pos[1]
ps.pose.position.z = photobox_pos[2] - wall_thickness / 2
scene.add_box('box_ground', ps,
(photobox_size[0], photobox_size[1], wall_thickness))
# add turntable
turntable_height = turntable_pos[2] - photobox_pos[2]
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = photobox_pos[2] + turntable_height / 2
scene.add_cylinder('turntable', ps, turntable_height, turntable_radius)
# add object on turntable
ps.pose.position.x = turntable_pos[0]
ps.pose.position.y = turntable_pos[1]
ps.pose.position.z = turntable_pos[2] + object_size[2] / 2
scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2)
# add cable mounts
scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount')
scene.remove_attached_object('forearm_link', 'forearm_cable_mount')
scene.remove_world_object('upper_arm_cable_mount')
scene.remove_world_object('forearm_cable_mount')
if ptpip is None and not simulation:
size = [0.08, 0.08, 0.08]
ps.header.frame_id = 'upper_arm_link'
ps.pose.position.x = -0.13
ps.pose.position.y = -0.095
ps.pose.position.z = 0.135
scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size)
ps.header.frame_id = 'forearm_link'
ps.pose.position.x = -0.275
ps.pose.position.y = -0.08
ps.pose.position.z = 0.02
scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size)
# add camera
eef_link = move_group.get_end_effector_link()
ps = PoseStamped()
ps.header.frame_id = eef_link
scene.remove_attached_object(eef_link, 'camera_0')
scene.remove_attached_object(eef_link, 'camera_1')
scene.remove_world_object('camera_0')
scene.remove_world_object('camera_1')
ps.pose.position.y = camera_pos[1]
ps.pose.position.z = camera_pos[2]
if camera_mesh:
ps.pose.position.x = camera_pos[0]
quaternion = tf.transformations.quaternion_from_euler(
math.radians(camera_orientation[0]),
math.radians(camera_orientation[1]),
math.radians(camera_orientation[2]))
ps.pose.orientation.x = quaternion[0]
ps.pose.orientation.y = quaternion[1]
ps.pose.orientation.z = quaternion[2]
ps.pose.orientation.w = quaternion[3]
scene.attach_mesh(eef_link, 'camera_0', ps,
os.path.expanduser(camera_mesh), camera_size)
if not simulation:
scene.attach_mesh(eef_link, 'camera_1', ps,
os.path.expanduser(camera_mesh),
numpy.array(camera_size) * 1.5)
vertices = scene.get_attached_objects(
['camera_0'])['camera_0'].object.meshes[0].vertices
camera_size[0] = max(vertice.x for vertice in vertices) - min(
vertice.x for vertice in vertices)
camera_size[1] = max(vertice.y for vertice in vertices) - min(
vertice.y for vertice in vertices)
camera_size[2] = max(vertice.z for vertice in vertices) - min(
vertice.z for vertice in vertices)
else:
ps.pose.position.x = camera_pos[0] + camera_size[0] / 2
scene.attach_box(eef_link, 'camera_0', ps, camera_size)
