def enable_cameras(self):
self.head_cam = baxter_interface.CameraController("head_camera")
self.left_hand_cam = baxter_interface.CameraController(
"left_hand_camera")
# -- camera options
# from url http://sdk.rethinkrobotics.com/wiki/Cameras
# Mode | Resolution | Width | Height
# 0 HIGH 1280 800
# 1 HIGH 960 600
# 2 HIGH 640 400
# 3 HIGH 480 300
# 4 HIGH 384 240
# 5 HIGH 320 200
# 6 LOW 640 400
# 7 LOW 480 300
# 8 LOW 384 240
# 9 LOW 320 200
# 10 LOW 240 150
# 11 LOW 192 120
image_option = 1
# self.head_cam.open()
self.left_hand_cam.resolution = (960, 600)
def main():
res = (1280,800)
# shutdown all cameras first
leftCam = baxter_interface.CameraController("left_hand_camera")
leftCam.close()
rightCam = baxter_interface.CameraController("right_hand_camera")
rightCam.close()
#headCam = baxter_interface.CameraController("head_camera")
#headCam.close()
# define Node
rospy.init_node('useCam')
#define argument for command line
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,description=main.__doc__)
parser.add_argument('-c', '--camera', choices=['left', 'right', 'head'], required=True,help="the camera to display")
args = parser.parse_args(rospy.myargv()[1:])
# define camera
if(args.camera == 'left'):
leftCam.open()
leftCam.resolution = res
camera_topic = '/cameras/left_hand_camera/image'
if(args.camera == 'right'):
rightCam.resolution = res
rightCam.open()
camera_topic = '/cameras/right_hand_camera/image'
#if(args.camera == 'head'):
# headCam.resolution = res
# headCam.open()
# camera_topic = '/cameras/head_camera/image'
stream2disp(camera_topic)
print("closing...")
return 0
def Camera_Control(self, camera_name, command):
assert (command =='open' or command=='close'), 'only two types of command are allowed. \n'
assert camera_name in camera_threads, 'Not given a valid camera name! \n'
def Parallel_Thread(string):
os.system(string)
ls = rospy.ServiceProxy('cameras/list', ListCameras)
rospy.wait_for_service('cameras/list', timeout=10)
resp = ls()
commands = {'left_hand_camera':"Thread(target=Parallel_Thread, args=('cd ../launch; roslaunch left_wrist.launch',))",
'right_hand_camera': "Thread(target=Parallel_Thread, args=('cd ../launch; roslaunch right_wrist.launch',))",
'head_camera': "Thread(target=Parallel_Thread, args=('cd ../launch; roslaunch head_cam.launch',))"}
if camera_name not in resp.cameras:
print('Cannot control ' + camera_name + ', other 2 cameras are open. ')
else:
if command == 'open':
if not camera_threads[camera_name]:
camera_threads[camera_name] = eval(commands[camera_name])
camera_threads[camera_name].start()
print(camera_name +' is opened.')
else:
print(camera_name + ' has already been launched!')
elif command == 'close':
print('I got your command!\n')
if camera_name == 'left_hand_camera':
camera = baxter_interface.CameraController('left_hand_camera')
elif camera_name == 'right_hand_camera':
camera = baxter_interface.CameraController('right_hand_camera')
else:
camera = baxter_interface.CameraController('head_camera')
if camera_threads[camera_name]:
print('Closing!!!!')
camera_threads[camera_name] = None
camera.close()
print(camera._id+' is closed.')
def init_camera(self):
self.ImageThreadLockLeft = threading.Lock()
self.ImageThreadLockRight = threading.Lock()
left_camera_sub = rospy.Subscriber('/cameras/left_hand_camera/image', \
Image, self._left_camera_callback)
right_camera_sub = rospy.Subscriber('/cameras/right_hand_camera/image', \
Image, self._right_camera_callback)
self.left_camera = self._camera = baxter_interface.CameraController(
'left_hand_camera')
self.right_camera = self._camera = baxter_interface.CameraController(
'right_hand_camera')
self.height = 600
self.width = 960
self.left_camera.open()
self.left_camera.resolution = [self.width, self.height]
self.left_camera.gain = 5
self.right_camera.open()
self.right_camera.resolution = [self.width, self.height]
self.right_camera.gain = 5
self.cam_calib = 0.0025 # meters per pixel at 1 meter
self.cam_x_offset = 0.045 # camera gripper offset
self.cam_y_offset = -0.01
self.left_cur_img = None #np.zeros((self.height, self.width, 3), np.uint8)
self.right_cur_img = np.zeros((self.height, self.width, 3), np.uint8)
def main():
try:
head_camera = baxter_interface.CameraController("head_camera")
print ("Attempting to turn off the head camera...")
head_camera.close()
except Exception:
pass
camera = baxter_interface.CameraController("right_hand_camera")
camera.open()
camera.resolution = [1280, 800]
camera.gain = 0
camera.exposure = 60
#Create names for OpenCV images and orient them appropriately
cv2.namedWindow("Sweets", 1)
#Initiate node for left hand camera
rospy.init_node('right_hand_camera', anonymous=True)
#Subscribe to left hand camera image
rospy.Subscriber("/cameras/right_hand_camera/image", Image, callback)
#Keep from exiting until this node is stopped
rospy.spin()
def __init__(self,arm):
# Define arm
self.limb = baxter_interface.Limb(arm)
# Define cameras
#self.resetCameras()
#self.closeCameras()
self.armCam = baxter_interface.CameraController('left_hand_camera')
self.armCam.resolution = (int(640),int(400))
self.armCam.open()
self.filmCam = baxter_interface.CameraController('right_hand_camera')
self.filmCam.resolution = (int(640),int(400))
self.filmCam.open()
# Define Gripper
self.wristAngle = arm + '_w2'
self.grip = baxter_interface.Gripper(arm)
self.grip.calibrate()
self.grip.set_holding_force(40)
# Topics
self.kinectTopic = '/transformedPointCloud'
self.camTopic = '/cameras/left_hand_camera/image'
self.filmTopic = '/cameras/right_hand_camera/image'
self.viewTopic = '/view'
self.viewImage = None
self.handImage = None
self.filmImage = None
self.feat = None
self.featStatic = None
self.featTopic = '/feature'
self.bridge = cv_bridge.CvBridge()
# Image
self.display = np.zeros((880,640+480,3), dtype = "uint8")
# Inverse kinematics
#self.IK = baxter_kinematics(arm)
#self.robot = moveit_commander.RobotCommander()
#self.scene = moveit_commander.PlanningSceneInterface()
#self.IKlimb = moveit_commander.MoveGroupCommander(arm+"_arm")
# Poses
# seed orientation pose
#self.a = -0.207902243738
#self.b = 0.977069770132
#self.c = -0.0233761695371
#self.d = 0.0395585141385
# move locations
self.start = {'left_w0': -0.220893233203125, 'left_w1': 1.7115390621276856, 'left_w2': 3.0564567163696292, 'left_e0': 0.22166022359619142, 'left_e1': 0.6074563913085937, 'left_s0': -0.33402431618041994, 'left_s1': -0.794602047216797}
self.mid = {'left_w0': -0.645422415765381, 'left_w1': 1.6854613887634278, 'left_w2': 3.0549227355834963, 'left_e0': -0.1219514724975586, 'left_e1': 0.5104321065856934, 'left_s0': -0.7907670952514649, 'left_s1': -0.9031311878356935}
self.box = {'left_w0': 1.010126347668457, 'left_w1': 1.6409759459655764, 'left_w2': 3.039582927722168, 'left_e0': -1.7257283843994142, 'left_e1': 2.0739420228515626, 'left_s0': -0.5729418236206055, 'left_s1': -0.37544179740600586}
#self.seed = {'left_w0': -0.10814564542236328, 'left_w1': 1.2808739564208986, 'left_w2': 0.26039323844604495, 'left_e0': -0.0019174759826660157, 'left_e1': 0.6166602760253906, 'left_s0': -0.9871166358764649, 'left_s1': -0.4475388943542481}
self.seed = np.array([ 0.87380432, 0.08285656, 0.15036509, 0.2033632 , 0.97567312,-0.05176495, 0.06344892])
# ROS stuff
self.rate = rospy.Rate(60)
self.z0 = 0.2
# start correct
self.limb.move_to_joint_positions(self.start)
# progress Flags
self.gotGrasp = 0
self.gotLoc = 0
self.gotFeat = 0
self.featLoc = (0,0)
self.still = 0
cv2.namedWindow('handImage')
def main(self):
head_camera = baxter_interface.CameraController("head_camera")
head_camera.close()
left_camera = baxter_interface.CameraController("left_hand_camera")
left_camera.resolution = (960, 600)
left_camera.open()
camera_name = "left_hand_camera"
sub = rospy.Subscriber('/cameras/' + camera_name + "/image", Image,
self.republish, None, 1)
rospy.spin()
def __init__(self):
# Instantiate all three cameras
self._left_camera = baxter_interface.CameraController(
'left_hand_camera')
self._right_camera = baxter_interface.CameraController(
'right_hand_camera')
self._head_camera = baxter_interface.CameraController('head_camera')
# Head screen publisher
self.image_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._bridge = CvBridge()
cv.NamedWindow("Image window", 1)
# Start left hand camera
self.start_camera()
def __init__(self, arm):
self.limb = arm
self.limb_interface = baxter_interface.Limb(
self.limb) #Declares the limb
self.head = baxter_interface.Head() #Declares the head
self.gripper = baxter_interface.Gripper(
self.limb) #Declares the gripper
self.camera = baxter_interface.CameraController('right_hand_camera')
self.camera.open()
self.camera.resolution = self.camera.MODES[0]
self.pub = rospy.Publisher(
'/robot/xdisplay', Image, latch=True,
queue_size=10) #Publisher for changing the screen display
#Declares the other arm (The opposite arm to the one passed as argument. If you pass left, declares right as the other arm and vice versa)
if arm == "left":
self.other_limb = "right"
else:
self.other_limb = "left"
self.other_limb_interface = baxter_interface.Limb(self.other_limb)
self.limb_interface.set_joint_position_speed(
0.5) #Sets the speed for the args arm. {0.0, 1.0}
self.other_limb_interface.set_joint_position_speed(
0.5) #Sets the speed for the other arm {0.0, 1.0}
self.angles = self.limb_interface.joint_angles(
) #Stores all the joint angles
def __init__(self, arm, counter):
self.arm = arm
self.current_poses = None
self.OcvBridge = CvBridge()
self.ImageThreadLock = threading.Lock()
self.ArmCmdPub = rospy.Publisher('robot_arms_cmd',
String,
queue_size=10)
self.ArmReply = ''
self.counter = counter
rospy.init_node("data_collecting")
camera_sub = rospy.Subscriber('/cameras/'+ arm + '_hand_camera/image', \
Image, self._camera_callback)
self.height = 600
self.width = 960
self._camera = baxter_interface.CameraController(self.arm +
'_hand_camera')
self._camera.open()
self._camera.resolution = [self.width, self.height]
self._camera.gain = 5
self.cam_calib = 0.0025 # meters per pixel at 1 meter
self.cam_x_offset = 0.045 # camera gripper offset
self.cam_y_offset = -0.01
def baxter_camera_open(self):
self.camera=baxter_interface.CameraController('head_camera')
self.camera_image = None
self.camera.resolution = (960, 600)
self.camera.fps = 120
self.camera.open()
def close_other_cameras(self):
"""Closes other cameras to accomodate for the bandwith limitation."""
for camera in self.other_cameras:
print camera
try:
b.CameraController(camera).close()
except AttributeError:
rospy.INFO("Tried to close {} and failed".format(camera))
def _reset_defaults():
"""Turn on the left/right_hand_cameras with default settings
"""
print("Restarting the Default Cameras...")
for i in range(1, 3):
camera = baxter_interface.CameraController(camera_names[i])
camera.resolution = (320, 200,)
camera.fps = 25
camera.open()
def _reset_defaults():
"""
Turn on the left/right_hand_cameras with default settings.
"""
print ("Restarting the Default Cameras...")
camera_list = _list_cameras()
for camera_name in camera_names:
print ("Restarting {0}".format(camera_name))
if not camera_name in camera_list.cameras:
# Attempt to close another camera
# and list services again
# (power to this camera was off)
baxter_interface.CameraController(camera_list.cameras[0]).close()
camera_list = _list_cameras()
camera = baxter_interface.CameraController(camera_name)
camera.resolution = (640, 400,)
camera.fps = 25
camera.open()
camera.close()
def __init__(self,name):
self._camcon = baxter_interface.CameraController(name+'_hand_camera')
self._resolution = (480,300) #可选的分辨率 1280x800,960x600,640x400,480x300,384x240
self._image_sub = rospy.Subscriber('/cameras/'+name+'_hand_camera/image', Image, self._image_callback)
self._original_image = None #从相机获取的原始图像(opencv格式)
self._MAX_SCALE = 100 # 找不到棋子就返回更大的值
self._pick_bias = (0.025,0.023) # the gripper is not at the center of the camera
# 颜色在HSV空间中的上下阈值 蓝色：[90,130,30],[130,200,150]v 148 128
self._color_dict = {'blue':[[70,80,30],[180,255,255]],'purple':[[130,50,0],[255,200,255]]} #125
self.image_updated = False
def get_baxter_camera_open(self):
#baxter_interface.CameraController('head_camera').close()
self.camera = baxter_interface.CameraController('head_camera')
self.camera_image = None
#self.camera.resolution = (320, 200)
#self.camera.resolution = (960, 600)
self.camera.resolution = (1280, 800)
self.camera.fps = 120
self.camera.open()
def __init__(self, baxter_name="Baxter", do_grippers=True):
rospy.init_node("Baxter_Node", anonymous=True)
# Give him a creative name
self.name = baxter_name
# Create head instance
try:
self.head = baxter_interface.Head()
except:
rospy.logerr('HEADS up: robot_interface could not find head.')
# Create baxter arm instances
self.right_arm = baxter_interface.Limb('right')
self.left_arm = baxter_interface.Limb('left')
self.BaxEnable = baxter_interface.RobotEnable()
# Create baxter gripper instances
if do_grippers:
self.right_gripper = baxter_interface.Gripper('right')
self.left_gripper = baxter_interface.Gripper('left')
self.right_gripper.calibrate()
self.left_gripper.calibrate()
# Create Baxter Hand Cameras
self.rhc = baxter_interface.CameraController('right_hand_camera')
self.lhc = baxter_interface.CameraController('left_hand_camera')
# Initialize camera (right only for now)
self.setCamera('right')
# Set up publishing to the face
rospack = rospkg.RosPack()
self.impath = rospack.get_path('sbb_hw5') + '/img/'
self.facepub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=10)
def __init__(self, limb_name):
self.limb_name = limb_name
self.arm = Arm(limb_name)
self.available_tread = True
if limb_name == 'left':
self.other_limb_name = 'right'
else:
self.other_limb_name = 'left'
self.otherArm = Arm(self.other_limb_name)
#self.otherArm.tuck_arm()
self.cards = np.array([])
self.bridge = CvBridge()
self.cards = Cards()
#rospy.init_node(limb_name, anonymous=True)
self.display_pub = rospy.Publisher('/robot/xdisplay', Image)
head_camera = baxter_interface.CameraController('head_camera')
head_camera.close()
self.other_camera = baxter_interface.CameraController(
self.other_limb_name + '_hand_camera')
self.other_camera.close()
self.camera = baxter_interface.CameraController(self.limb_name +
'_hand_camera')
self.camera.open()
self.arm.calibrate()
resp = 'n'
while resp is not 'y':
resp = str(
raw_input(
'Please place the gripper arm at the center of the card holder, press "y" to continue: '
))
self.arm.go_home()
if self.available_tread:
cam_thread = threading.Thread(target=self.get_camera)
choices_thread = threading.Thread(target=self.pick_choices)
cam_thread.start()
choices_thread.start()
rospy.spin()
cv2.destroyAllWindows()
def main(args):
rospy.init_node('imgConv', anonymous=True)
#limb = baxter_interface.Limb('left')
res = (1280,800)
# define cams
leftCam = baxter_interface.CameraController("left_hand_camera")
rightCam = baxter_interface.CameraController("right_hand_camera")
#headCam = baxter_interface.CameraController("head_camera")
#define argument for command line
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,description=main.__doc__)
parser.add_argument('-c', '--camera', choices=['left', 'right', 'head'], required=True,help="the camera to display")
args = parser.parse_args(rospy.myargv()[1:])
# define camera
if(args.camera == 'left'):
leftCam.open()
leftCam.resolution = res
camera_topic = '/cameras/left_hand_camera/image'
if(args.camera == 'right'):
rightCam.resolution = res
rightCam.open()
camera_topic = '/cameras/right_hand_camera/image'
#if(args.camera == 'head'):
# headCam.resolution = res
# headCam.open()
# camera_topic = '/cameras/head_camera/image'
stream(camera_topic)
#pose = limb.endpoint_pose()
#x,y,z = pose['position']
#print x,y,z
print 'Closed'
cv2.destroyAllWindows()
return 0
def __init__(self, camera_name, mode, half_res):
print "Initializing ROS Node"
rospy.init_node('baxter_cameras', anonymous=True)
# Lock for multithreading
self._lock = threading.RLock()
# for image pipe
self._imagestream = None
self._imagestream_endpoints = dict()
self._imagestream_endpoints_lock = threading.RLock()
# get access to camera controls from RSDK
self._camera = baxter_interface.CameraController(camera_name)
self._camera_name = camera_name
# automatically close camera at start
# self._camera.close()
self._camera_open = False
# set constant ImageHeader structure
self.setResolution(mode, half_res)
self._image_header = RR.RobotRaconteurNode.s.NewStructure(
"BaxterCamera_interface.ImageHeader")
self._image_header.width = int(self._camera.resolution[0])
self._image_header.height = int(self._camera.resolution[1])
self._image_header.step = int(4)
self._camera_intrinsics = None
# set exposure, gain, white_balance to auto
self._camera.exposure = self._camera.CONTROL_AUTO
self._camera.gain = self._camera.CONTROL_AUTO
self._camera.white_balance_red = self._camera.CONTROL_AUTO
self._camera.white_balance_green = self._camera.CONTROL_AUTO
self._camera.white_balance_blue = self._camera.CONTROL_AUTO
# set BaxterImage struct
self._image = RR.RobotRaconteurNode.s.NewStructure(
"BaxterCamera_interface.BaxterImage")
self._image.width = self._image_header.width
self._image.height = self._image_header.height
self._image.step = self._image_header.step
# Initialize ARtag detection
self._aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
self._arucoParams = aruco.DetectorParameters_create()
self._markerSize = 0.085
def __init__(self, img_width, img_height, side):
self.cur_img = None
self.OcvBridge = CvBridge()
self.height = img_height
self.width = img_width
camera_name = side + '_hand_camera'
self._camera = baxter_interface.CameraController(camera_name)
self._camera.open()
self._camera.resolution = [self.width, self.height]
self._camera.gain = 10
self.ImageThreadLock = threading.Lock()
camera_topic_name = '/cameras/' + side + '_hand_camera/image'
camera_sub = rospy.Subscriber(camera_topic_name, \
Image, self._camera_callback)
pass
def __init__(self, limb, distance, loops, calibrate):
# Create baxter_interface limb instance
self._arm = limb
self._limb = baxter_interface.Limb(self._arm)
self._gripper = baxter_interface.Gripper(self._arm)
self._cam = baxter_interface.CameraController(self._arm +
'_hand_camera')
self._cam.gain = 1
self._ik_srv = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(self._ik_srv, SolvePositionIK)
self._ikreq = SolvePositionIKRequest()
self._hover_distance = distance
self._num_loops = loops
self._domino_source_approach = None
self._domino_source = None
self._domino_source_jp = None
self._calibration_point = None
# Recorded waypoints
self._waypoints = list()
self._source_point = None
# Recording state
self._is_recording = False
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# Calibrate Gripper
if (calibrate):
self._gripper.calibrate()
if not (self._gripper.calibrated() or self._gripper.calibrate()):
rospy.logwarn("%s (%s) calibration failed.",
self._gripper.name.capitalize(),
self._gripper.type())
# Create Navigator I/O
self._navigator_io = baxter_interface.Navigator(self._arm)
self._overhead_orientation = Quaternion(x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
def __init__(self):
"""
Construtor: initializes node, subscribe to Baxter's right hand camera
topic and create the service.
"""
self.lastImage = None;
rospy.init_node('right_hand_camera')
cameraController = baxter_interface.CameraController("right_hand_camera")
#cameraController.close()
#cameraController.resolution= (1280,800)
#cameraController.open()
rospy.Subscriber("/cameras/right_hand_camera/image", Image, self.imgReceived)
#self.pub = rospy.Publisher('/image_cmd', Image, queue_size=10)
rospy.Service('last_image', ImageSrv, self.getLastImage)
def __init__(self, sim=False):
"""Hardware abstraction of the Baxter robot using the BaxterSDK
interface.
:param sim: Whether in Gazebo (True) or on real Baxter (False).
"""
name = 'main.baxter'
self._logger = logging.getLogger(name)
self._arms = ['left', 'right']
self._limbs = {a: baxter_interface.Limb(a)
for a in self._arms}
self._grippers = {a: baxter_interface.Gripper(a)
for a in self._arms}
self._grippers_pars = self._grippers['left'].valid_parameters()
self._grippers_pars['moving_force'] = 40.0
self._grippers_pars['holding_force'] = 30.0
self._sensors = {a: baxter_interface.analog_io.AnalogIO('%s_hand_range' % a)
for a in self._arms}
# Cameras on the Baxter robot are tricky. Due to limited bandwidth
# only two cameras can be operating at a time.
# http://sdk.rethinkrobotics.com/wiki/Camera_Control_Tool
# Default behavior on Baxter startup is for both of the hand cameras
# to be in operation at a resolution of 320x200 at a frame rate of
# 25 fps. We get their CameraControllers using the Baxter SDK ...
self.cameras_d = {a: baxter_interface.CameraController('%s_hand_camera' % a, sim=sim)
for a in self._arms}
# ... and set their resolution to 1280x800 @ 14 fps.
for arm in self._arms:
self.cameras_d[arm].resolution = (1280, 800)
self.cameras_d[arm].fps = 14.0
self.cameras_d[arm].exposure = settings.baxter_cam_exposure
# We don't need the CameraControllers any more. Our own module will
# do the remaining camera handling for us.
self.cameras = {a: Camera(topic='/cameras/{}_hand_camera/image'.format(a),
prefix=name)
for a in self._arms}
self._planner = SimplePlanner()
self._rs = None
self._init_state = None
self.cam_offset = None
self.range_offset = None
self.z_table = None
def __init__(self):
self.UsingSavedImage = False
self.ImageCounter = 0
self.cur_img = None
self.ImageThreadLock = threading.Lock()
self.current_poses = None
self.OcvBridge = CvBridge()
left_camera_sub = rospy.Subscriber('/cameras/left_hand_camera/image', \
Image, self._camera_callback)
self.height = 600
self.width = 960
self._camera = baxter_interface.CameraController('left_hand_camera')
self._camera.open()
self._camera.resolution = [self.width, self.height]
self._camera.gain = 16
#self.cam_calib = 0.0025 # meters per pixel at 1 meter
#self.cam_x_offset = 0.045 # camera gripper offset
#self.cam_y_offset = -0.01
self.ImageFolder = "/home/robo-01/ros_ws/src/phm/camera_calibration/"
left_ir_msg = message_filters.Subscriber(
'/robot/range/left_hand_range/state', Range)
right_ir_msg = message_filters.Subscriber(
'/robot/range/right_hand_range/state', Range)
ts = message_filters.ApproximateTimeSynchronizer(
[left_ir_msg, right_ir_msg], 10, 0.05)
ts.registerCallback(self._ir_sensor_callback)
self.current_ir_ranges = {'left': 65.0, 'right': 65.0}
left_arm_msg = message_filters.Subscriber(
"/robot/limb/left/endpoint_state", EndpointState)
right_arm_msg = message_filters.Subscriber(
"/robot/limb/right/endpoint_state", EndpointState)
ts = message_filters.ApproximateTimeSynchronizer(
[left_arm_msg, right_arm_msg], 10, 0.05)
ts.registerCallback(self._pose_callback)
self.current_poses
def cam_setup(self):
cam_name = self.limb_side + "_hand_camera"
cam_controller = baxter_interface.CameraController(cam_name)
cam_controller.resolution = (640, 400)
redo = 'y'
#while (redo == 'y'):
while (1):
exposure = cam_controller.exposure
gain = cam_controller.gain
wb_g = cam_controller.white_balance_green
wb_b = cam_controller.white_balance_red
wb_r = cam_controller.white_balance_blue
print("Current exposure/gain: " + str(exposure) + "/" + str(gain) +
"/" + str(wb_r) + "/" + str(wb_g) + "/" + str(wb_b))
settings = input(" New settings: ")
cam_controller.exposure = settings[0]
cam_controller.gain = settings[1]
cam_controller.white_balance_red = settings[2]
cam_controller.white_balance_green = settings[3]
cam_controller.white_balance_blue = settings[4]
def __init__(self, side='left', do_grippers=True):
self.side = side
# Create Baxter arm instance
self.limb = baxter_interface.Limb(side)
self.BaxEnable = baxter_interface.RobotEnable()
# Create Baxter gripper instance
if do_grippers:
self.gripper = baxter_interface.Gripper(side)
self.gripper.calibrate()
# Create Baxter camera instance
self.cam = baxter_interface.CameraController(side + '_hand_camera')
# Initialize camera
self.setCamera(side)
# Baxter kinematics object
self.bk = baxter_pykdl.baxter_kinematics(side)
# Subscribe to the cuff button
rospy.Subscriber('/robot/digital_io/' + side + '_lower_button/state',
DigitalIOState, self.storeCuff)
# We'll be controlling joints directly with messages to the joint control topic
self.cmd = rospy.Publisher('robot/limb/' + side + '/joint_command',
JointCommand,
queue_size=10)
# Set the initial joint command message to what we have
self.cartVel = False
self.setJoints(self.getJoints())
# Start up the thread that periodically commands the arm
self.t = threading.Thread(target=self.jointPublisher)
self.t.daemon = True
self.t.start()
def __init__(self, limb, verbose, arm_z=5.0):
"""
Constructor for the Cam class.
Parameters:
limb: The limb of Baxter whichs camera shall be managed; Options: 'left', 'right'
verbose: True -> print verbose information; False -> dont print verbose information; Default: False
arm_z: z value of the cameras arm in meter (needed for action point prediction); Default: 5.0
"""
sub_cam = "/cameras/{}_hand_camera/image".format(limb)
self._limb = limb
self.controller = baxter_interface.CameraController("{}_hand_camera".format(limb))
self.controller.resolution = (640, 400)
self.bridge = CvBridge()
self.update_snapshot = True
self._verbose = verbose
self._init = True
self.sub = rospy.Subscriber(sub_cam, Image, self.show_callback)
self.arm_z = arm_z
self.windowed = False
self._img = None
self.ix = 0
def __init__(self, limb='left', moveToInitial=True, device=0):
# self.is_stopping = False
rospy.loginfo('Starting the 3DOF controller for the %s limb', limb)
rospy.init_node("dqn_baxter")
self.rate = rospy.Rate(30) # 10 Hz
self.limb = limb
self.joint_names = {'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2'}
self.index_corr = {'left_s0': 1, 'left_s1': 2, 'left_e0': 3, 'left_e1': 4, 'left_w0': 5, 'left_w1': 6, 'left_w2': 7} # the index correspondence
self.cam_joint_names = {'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'}
self.cam_index_corr = {'right_s0': 1, 'right_s1': 2, 'right_e0': 3, 'right_e1': 4, 'right_w0': 5, 'right_w1': 6, 'right_w2': 7}
# self.joint_names = { 'left_w1', 'left_e1', 'left_s1' }
# self.delta_angle = 0.04 # Baxter joint angles are in radians
#self.delta_angle = 0.1 # used in the last test
# active Baxter
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
self.interface = baxter_interface.Limb(limb)
self.camera_arm = baxter_interface.Limb('right')
self.cam = baxter_interface.CameraController('right_hand_camera')
self.head = baxter_interface.Head()
self.bridge = cv_bridge.CvBridge()
self.curr_im = np.zeros((3,480,640))
self.image_sub = rospy.Subscriber("/cameras/right_hand_camera/image",Image,self.updateCurrRGBImage)
# magic constant: joint speed :)
# self.interface.set_joint_position_speed(0.9)
# if moveToInitial: self.initCam()
# initialize the desired position with the current position
# angles = self.interface.joint_angles()
# self.desired_joint_pos = { key: angles[key] for key in self.joint_names }
rospy.loginfo('Initialization finished!')
def __init__(self, arm):
self.arm = arm
self.GridLocations = []
self.GridRoiPose = None
self.GridRois = []
self.current_poses = None
self.OcvBridge = CvBridge()
self.cur_img = None
self.ImageThreadLock = threading.Lock()
self.ArmCmdPub = rospy.Publisher('robot_arms_cmd',
String,
queue_size=10)
camera_sub = rospy.Subscriber('/cameras/'+ self.arm + '_hand_camera/image', \
Image, self._camera_callback)
self.height = 600
self.width = 960
self._camera = baxter_interface.CameraController(self.arm +
'_hand_camera')
self._camera.open()
self._camera.resolution = [self.width, self.height]
self._camera.gain = 10
rospy.init_node("game_calibrator")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
left = baxter_interface.Gripper('left', CHECK_VERSION)
right = baxter_interface.Gripper('right', CHECK_VERSION)
self.baxter_grippers = {'left': left, 'right': right}
self.mx = 0
self.my = 0
self.GridRoiDoneFlag = False
self.GridRoiCounter = 0
self.TempGridList = []
