def initData(self): self.base_frame = "/base" self.rgb_camera_frame = "/camera_carmine_1_rgb_optical_frame" self.tf_listener = tf.TransformListener() self.hypothesis_k = 0 self.current_depth_image = None self.camera_base_channel = "/camera_carmine_1" self.registered_cloud_subscriber = ros_utils.SimpleSubscriber( self.camera_base_channel + "/depth_registered/points", sensor_msgs.msg.PointCloud2) self.rgb_camera_info_subscriber = ros_utils.SimpleSubscriber( self.camera_base_channel + "/depth_registered/sw_registered/camera_info", sensor_msgs.msg.CameraInfo) all_subscribers = [ self.registered_cloud_subscriber, self.rgb_camera_info_subscriber ] for subscriber in all_subscribers: subscriber.start(queue_size=1) rospy.loginfo("Waiting on topic %s..." % (subscriber.topic)) subscriber.waitForNextMessage() rospy.loginfo("All channels are alive.") camera_matrix_msg = self.rgb_camera_info_subscriber.waitForNextMessage( ) self.camera_matrix = np.array(camera_matrix_msg.K).reshape(3, 3) self.hypotheses = []
def setupSubscribers(self): self.subscribers = dict() self.subscribers['rgb'] = rosUtils.SimpleSubscriber( self.topics_dict['rgb'], sensor_msgs.msg.Image) self.subscribers['depth'] = rosUtils.SimpleSubscriber( self.topics_dict['depth'], sensor_msgs.msg.Image)
def testROS(self): topic = self.config['rgb_raw_topic'] msgType = sensor_msgs.msg.Image self.simpleSubscriber = spartanROSUtils.SimpleSubscriber(topic, msgType) self.taskRunner.callOnThread(self.simpleSubscriber.start) topic = "/joint_states" msgType = sensor_msgs.msg.JointState self.jointStatesSubscriber = spartanROSUtils.SimpleSubscriber(topic, msgType) self.taskRunner.callOnThread(self.jointStatesSubscriber.start)
def setupSubscribers(self): self.subscribers = {} self.subscribers['rgb'] = rosUtils.SimpleSubscriber( self.topics_dict['rgb'], sensor_msgs.msg.Image, externalCallback=self.onRgbImage) self.subscribers['depth'] = rosUtils.SimpleSubscriber( self.topics_dict['depth'], sensor_msgs.msg.Image, externalCallback=self.onDepthImage)
def setupSubscribers(self): self.pointCloudSubscriber = rosUtils.SimpleSubscriber(self.pointCloudTopic, sensor_msgs.msg.PointCloud2) self.rgbImageSubscriber = rosUtils.SimpleSubscriber(self.rgbImageTopic, sensor_msgs.msg.Image) self.depthImageSubscriber = rosUtils.SimpleSubscriber(self.depthImageTopic, sensor_msgs.msg.Image) self.camera_info_subscriber = rosUtils.SimpleSubscriber(self.camera_info_topic, sensor_msgs.msg.CameraInfo) self.pointCloudSubscriber.start() self.rgbImageSubscriber.start() self.depthImageSubscriber.start() self.camera_info_subscriber.start() self.clicked_point_subscriber = rosUtils.SimpleSubscriber("/clicked_point", geometry_msgs.msg.PointStamped, self.on_clicked_point) self.clicked_point_subscriber.start()
def startImageSubscribers(self): self.imageSubscribers = dict() msgType = sensor_msgs.msg.Image for key, topic in self.imageTopics.iteritems(): sub = rosUtils.SimpleSubscriber(topic, msgType) sub.start() self.imageSubscribers[key] = sub
def __init__(self, config, timestep, rate, rgbd_noise=0.005, rgbd_normal_limit=0.05, rgbd_projector_baseline=0.1, camera_serial_number="carmine_1"): self.config = config self.timestep = timestep self.rate = rate self.camera_serial_number = camera_serial_number self.packageMap = PackageMap() self.packageMap.populateFromEnvironment(["ROS_PACKAGE_PATH"]) self.iiwa_command_lock = threading.Lock() self.last_iiwa_position_command = [0.] * len(IIWA_CONTROLLED_JOINTS) self.schunk_command_lock = threading.Lock() self.last_schunk_position_command = [0.] * len(SCHUNK_CONTROLLED_JOINTS) self.last_schunk_torque_command = [0.] * len(SCHUNK_CONTROLLED_JOINTS) # Set up IIWA command subscriber over LCM self.lc = lcm.LCM() self.iiwa_command_sub = self.lc.subscribe("IIWA_COMMAND", self.HandleIiwaCommand) # Set up Schunk command subscriber on ROS self.schunk_command_sub = rosUtils.SimpleSubscriber("/schunk_driver/schunk_wsg_command", wsg50_msgs.msg.WSG_50_command, self.HandleSchunkCommand) self.schunk_command_sub.start(queue_size=1) self.schunk_status_publisher = rospy.Publisher("/schunk_driver/schunk_wsg_status", wsg50_msgs.msg.WSG_50_state, queue_size=1) # Set up image publishing camera_topics = RgbdCameraMetaInfo.getCameraPublishTopics(self.camera_serial_number) self.rgb_publisher = rospy.Publisher(camera_topics['RGB_TOPIC'], sensor_msgs.msg.Image, queue_size=1) self.depth_publisher = rospy.Publisher(camera_topics['DEPTH_TOPIC'], sensor_msgs.msg.Image, queue_size=1) self.rgb_info_publisher = rospy.Publisher(camera_topics['RGB_INFO_TOPIC'], sensor_msgs.msg.CameraInfo, queue_size=1) self.depth_info_publisher = rospy.Publisher(camera_topics['DEPTH_INFO_TOPIC'], sensor_msgs.msg.CameraInfo, queue_size=1) self.cv_bridge = CvBridge() self.rgbd_projector_baseline = rgbd_projector_baseline self.rgbd_normal_limit = rgbd_normal_limit self.rgbd_noise = rgbd_noise # Set up a Remote Tree Viewer wrapper to publish # object states self.rtv = Rtv.RemoteTreeViewerWrapper()
def add_subscriber(self, topic, name=None, call_in_thread=True, visualize=False, msg_type=None): """ Adds a subscriber :param topic: :type topic: :param call_in_thread: :type call_in_thread: :return: :rtype: """ if call_in_thread: self.add_subscriber(topic, name=name, call_in_thread=False, visualize=visualize, msg_type=msg_type) return if msg_type is None: msg_type = sensor_msgs.msg.PointCloud2 if name is None: name = topic subscriber = ros_utils.SimpleSubscriber(topic, msg_type) subscriber.start() d = dict() d['subscriber'] = subscriber d['topic'] = topic d['visualize'] = visualize d['name'] = name self._subscribers[topic] = d
def setupSubscribers(self): self.pointCloudSubscriber = rosUtils.SimpleSubscriber(self.pointCloudTopic, sensor_msgs.msg.PointCloud2) self.pointCloudSubscriber.start()
def runROSCalibration(self, headerData): headerData['target'][ 'location_estimate_in_robot_base_frame'] = self.calibrationPosesConfig[ 'target_location'] calibrationRunData = dict() calibrationRunData['header'] = headerData #setup our passive subscribers to the IR or RGB data topicName = None self.passiveSubscriber = None if self.captureRGB: topicName = self.config['rgb_raw_topic'] if self.captureIR: topicName = self.config['ir_raw_topic'] if topicName: self.passiveSubscriber = spartanROSUtils.SimpleSubscriber( topicName, sensor_msgs.msg.Image) self.passiveSubscriber.start() unique_name = time.strftime( "%Y%m%d-%H%M%S") + "_" + self.calibrationType self.calibrationFolderName = os.path.join( spartanUtils.getSpartanSourceDir(), 'calibration_data', unique_name) os.system("mkdir -p " + self.calibrationFolderName) os.chdir(self.calibrationFolderName) poseDict = self.computeCalibrationPoses() self.drawResult(poseDict) rospy.loginfo("finished making calibration poses") rospy.loginfo("starting calibration run") calibrationData = [] # topic = self.config['rgb_raw_topic'] # msgType = sensor_msgs.msg.Image # self.subscribers = dict() # self.subscribers[topic] = spartanROSUtils.SimpleSubscriber(topic, msgType) # self.subscribers[topic].start() for pose in poseDict['feasiblePoses']: # move robot to that joint position rospy.loginfo("\n moving to pose") self.robotService.moveToJointPosition(pose['joint_angles']) rospy.loginfo("capturing images and robot data") data = self.captureCurrentRobotAndImageData( captureRGB=self.captureRGB, captureIR=self.captureIR) calibrationData.append(data) rospy.loginfo("finished calibration routine, saving data to file") calibrationRunData['data_list'] = calibrationData spartanUtils.saveToYaml( calibrationRunData, os.path.join(self.calibrationFolderName, 'robot_data.yaml')) if self.passiveSubscriber: self.passiveSubscriber.stop() return calibrationRunData
ee_frame_id="iiwa_link_ee") # TODO: deal with rotations not being right goal.gains.append(make_cartesian_gains_msg(0., 20.)) goal.force_guard.append(make_downward_force_guard_msg(25.)) return goal # Useful scanning positions central_view_pose = np.array([4.9, 13.7, -1.0, -95.5, -3.7, 51.4, 29.3 ]) * np.pi / 180. if __name__ == "__main__": rospy.init_node('carrot_collector', anonymous=True) robotSubscriber = JointStateSubscriber("/joint_states") configBundleSubscriber = rosUtils.SimpleSubscriber( "/carrot_configs", carrot_msgs.msg.CarrotConfigurationBundle) configBundleSubscriber.start() rospy.loginfo("Set up subscribers...") rospy.sleep(1.0) # EE CONTROL VERSION client = actionlib.SimpleActionClient( "plan_runner/CartesianTrajectory", robot_msgs.msg.CartesianTrajectoryAction) rospy.loginfo("Waiting for action servers...") client.wait_for_server() rospy.loginfo("Waiting for hearing full robot state...") while len(robotSubscriber.joint_positions.keys()) < 3: rospy.sleep(0.1)
def setupSubscribers(self, statusSubscriberCallback): self.statusSubscriber = rosUtils.SimpleSubscriber( self.statusTopic, wsg50_msgs.msg.WSG_50_state, statusSubscriberCallback) self.statusSubscriber.start(queue_size=1)
def setupSubscribers(self): self.lastStatusMsg = None self.statusSubscriber = rosUtils.SimpleSubscriber(self.statusTopic, wsg_50_common.msg.Status) self.statusSubscriber.start(queue_size=1)
def runROSCalibrationFixedMount(self, headerData, topic_name, poses_file): headerData['target']['transform_to_hand_frame'] = dict() headerData['target']['transform_to_hand_frame']['translation'] = dict() headerData['target']['transform_to_hand_frame']['translation'][ 'x'] = 0.0 headerData['target']['transform_to_hand_frame']['translation'][ 'y'] = 0.0 headerData['target']['transform_to_hand_frame']['translation'][ 'z'] = 0.0931 headerData['target']['transform_to_hand_frame']['quaternion'] = dict() headerData['target']['transform_to_hand_frame']['quaternion'][ 'x'] = 0.49894425 headerData['target']['transform_to_hand_frame']['quaternion'][ 'y'] = 0.50080633 headerData['target']['transform_to_hand_frame']['quaternion'][ 'z'] = 0.51116578 headerData['target']['transform_to_hand_frame']['quaternion'][ 'w'] = -0.48883249 self.calibrationData = dict() calibrationRunData = self.calibrationData calibrationRunData['header'] = headerData self.passiveSubscriber = spartanROSUtils.SimpleSubscriber( topic_name, sensor_msgs.msg.Image) self.passiveSubscriber.start() unique_name = time.strftime( "%Y%m%d-%H%M%S") + "_" + self.calibrationType self.calibrationFolderName = os.path.join( spartanUtils.getSpartanSourceDir(), 'calibration_data', unique_name) os.system("mkdir -p " + self.calibrationFolderName) os.chdir(self.calibrationFolderName) rospy.loginfo("starting calibration run") calibrationData = [] poses = spartanUtils.getDictFromYamlFilename(poses_file) num_poses = len(poses) for index, pose_name in enumerate(sorted(poses.keys())): # move robot to that joint position info_msg = "\n moving to pose " + str(index) + " of " + str( num_poses) + " named " + pose_name rospy.loginfo(info_msg) self.robotService.moveToJointPosition(poses[pose_name]) rospy.loginfo("capturing images and robot data") data = self.captureCurrentRobotAndImageData( captureRGB=self.captureRGB, captureIR=self.captureIR, prefix=str(index)) calibrationData.append(data) rospy.loginfo("finished calibration routine, saving data to file") calibrationRunData['data_list'] = calibrationData spartanUtils.saveToYaml( calibrationRunData, os.path.join(self.calibrationFolderName, 'robot_data.yaml')) if self.passiveSubscriber: self.passiveSubscriber.stop() self.moveHome() return calibrationRunData
def makeSubscriber(topic, packageName, msgName): msgType = getMessageType(packageName, msgName) subscriber = spartanROSUtils.SimpleSubscriber(topic, msgType) subscriber.start() return subscriber
def setupSubscribers(self): self.lastStatusMsg = None self.statusSubscriber = rosUtils.SimpleSubscriber( self.statusTopic, wsg50_msgs.msg.WSG_50_state, self.handleStatusCallback) self.statusSubscriber.start(queue_size=1)