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)
