in src/rosbridge/fetch_proxy.py [0:0]
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')