def put_command_callback(self, msg): """ Attempts to put whatever is currently grasped / point claw at the position of a body with a given offset """ print "I am here..." # Attempt to get the body body = self.arm.env.GetKinBody(msg.object_id) if body == None: self.put_response_pub.publish(msg.object_id, OBJECT_NOT_LOADED) print "object not loaded" return # Offset body_info = openraveLab.decomposeTransform(body.GetTransform()) x = msg.x_offset + body_info[0] y = msg.y_offset + body_info[1] z = msg.z_offset + body_info[2] # Move hand try: self.arm.moveTo(x, y, z, msg.roll, msg.pitch, msg.yaw) self.waitrobot() except: self.put_response_pub.publish(msg.object_id, PLANNING_FAILED) print "planning failed" return # Attempt to release if msg.release: self.arm.open() # Report back self.put_response_pub.publish(msg.object_id, SUCCESS) print "success"
def body_request_callback(self, msg): """ Responds to a BodyStatusRequest with a BodyStatus response """ kinbody = self.arm.env.GetKinBody(msg.object_id) if(kinbody == None): self.body_status_pub.publish(BodyStatus(msg.object_id, False, 0, 0, 0, OBJECT_NOT_LOADED)) else: # Publish its current state info = openraveLab.decomposeTransform(kinbody.GetTransform()) self.body_status_pub.publish(BodyStatus(msg.object_id, kinbody.IsAttached(self.arm.robot), info[0], info[1], info[2], SUCCESS))
def get_camera_position(self):
""" Returns the camera's position / orientation as a simple stat structure """
camera_transform = self.arm.robot.GetAttachedSensor("sensor0").GetTransform() # TODO: sensor0, really? I call myself a college CS student?
info = openraveLab.decomposeTransform(camera_transform)
return State(info[0], info[1], info[2], info[3], info[4], info[5])
def hand_request_callback(self, msg):
""" Callback for the request topic for the location of the hand """
arm = self.arm.env.GetKinBody("pdArm")
transform = arm.GetJoint("JLR").GetFirstAttached().GetTransform() # Get the joint with the hand on it and then get the hand
info = openraveLab.decomposeTransform(transform)
self.hand_pub.publish(HandState(info[0], info[1], info[2], info[3], info[4], info[5], self.arm.fingersClosed))
