def __init__(self):
self.dopeCollection = DopeCollection()
self.forwardImage = ForwardImage(
input_camera_raw=rosparamOrDefault('~camera_raw',
'/d435_sim/camera_raw'),
input_camera_info=rosparamOrDefault('~camera_info',
'/d435_sim/camera_info'),
output_camera_raw=rosparamOrDefault('~dope_camera_raw',
'/dope/camera_raw'),
output_camera_info=rosparamOrDefault('~dope_camera_info',
'/dope/camera_info'))
# Setup service
self.service = rospy.Service(
rosparamOrDefault('~object_request_service', '/object_request'),
triple_s_util.srv.ObjectRequest, self.onObjectRequest)
if not rospy.has_param('/dope/class_ids'):
rospy.logerr(
'FindObject: /dope/class_ids is not loaded on the parameter server!'
)
sys.exit(1)
self.classIds = rospy.get_param('/dope/class_ids')
rospy.loginfo('Done initializing find_object.py')
def main():
""" Initialize the nodes """
global publisher, topic, frame_id
rospy.init_node('set_reference_frame', anonymous=True)
rospy.loginfo('Starting set_marker_frame.py')
topic = '/' + rosparamOrDefault('/dope/topic_publishing',
'dope') + '/markers'
frame_id = rosparamOrDefault('/bin_picking/camera_link', 'camera_sim_link')
rospy.Subscriber(topic, MarkerArray, onMarkerReceived)
publisher = rospy.Publisher(topic + '_fixed', MarkerArray, queue_size=10)
rospy.spin()
def getRandomPose(self):
"""
Generate a random pose
return -- geometry_msgs/PoseStamped
"""
pose = geometry_msgs.msg.PoseStamped()
pose.header.frame_id = rosparamOrDefault(
'/bin_picking/pose_reference_frame', 'base_link')
pose.pose.position.x = random.uniform(self.min_x_object,
self.max_x_object)
pose.pose.position.y = random.uniform(self.min_y_object,
self.max_y_object)
pose.pose.position.z = random.uniform(self.min_z_object,
self.max_z_object)
quat = Quaternion.random()
pose.pose.orientation.x = quat[1]
pose.pose.orientation.y = quat[2]
pose.pose.orientation.z = quat[3]
pose.pose.orientation.w = quat[0]
return pose
def sequence(self):
object_to_request = 'tomatosauce'
# Temporary: Move to start position
rospy.loginfo('Moving to start position')
self.planner.planAndExecuteNamedTarget('dope_to_rviz')
# Request a pose of an object through the service
rospy.loginfo('Requesting object position')
request = self.requestObjectPose(object_to_request)
rospy.loginfo('Got object position')
if request.found_object:
# Set the reference frame of the pose to the camera
request.object_pose.header.frame_id = rosparamOrDefault(
'~camera_link', 'camera_sim_link')
if not self.planner.planAndExecuteInReferenceFrame(
request.object_pose):
rospy.logwarn(
'Couldn\'t move into position to grab the object!')
else:
rospy.loginfo('Moved to object')
else:
rospy.logwarn('Couldn\'t find any objects of type \"%s\"' %
object_to_request)
# Sleep for a bit
rospy.sleep(10)
# Repeat
self.sequence()
def __init__(self):
self.planner = Planner()
object_request_service_name = rosparamOrDefault(
'~object_request_service', '/object_request')
rospy.wait_for_service(object_request_service_name)
self.requestObjectPose = rospy.ServiceProxy(
object_request_service_name, triple_s_util.srv.ObjectRequest)
self.sequence()
def publishPose(self, pose, publisher):
"""
Publish a pose message on a PoseStamped topic
"""
poseStamped = geometry_msgs.msg.PoseStamped()
poseStamped.header.frame_id = rosparamOrDefault(
'/bin_picking/pose_reference_frame', 'base_link')
poseStamped.pose = pose
publisher.publish(poseStamped)
def chooseBestDetection(self, detections):
""" Find the best pose to move to"""
if len(detections) > 0:
best_object = None
best_object_pose = None
for detection in detections:
object_pose = geometry_msgs.msg.PoseStamped()
object_pose.header.frame_id = rosparamOrDefault(
'/bin_picking/camera_link', 'camera_sim_link')
object_pose.pose = detection.results[0].pose.pose
success, pose = self.transformToReferenceFrame(object_pose)
if not success:
continue
x = pose.pose.position.x
y = pose.pose.position.y
z = pose.pose.position.z
if x >= self.min_x_object and x <= self.max_x_object \
and y >= self.min_y_object and y <= self.max_y_object \
and z >= self.min_z_object and z <= self.max_z_object:
if best_object == None:
best_object = detection
best_object_pose = pose
else:
if best_object_pose.pose.position.z < z:
best_object = detection
best_object_pose = pose
if best_object is not None:
best_object.results[0].pose.pose = best_object_pose.pose
best_object.header.frame_id = self.pose_reference_frame
return best_object
else:
return None
def __init__(self):
self.planner = Planner()
self.approach_distance = 0.2
self.pick_up_config = rosparamOrDefault(
'/bin_picking/cylindrical_axis', {})
self.z_limit = rosparamOrDefault('/bin_picking/rodrigues_z_limit', 0.8)
self.rodrigues_resolution = rosparamOrDefault(
'/bin_picking/rodrigues_resolution', 100)
self.pose_reference_frame = rosparamOrDefault(
'/bin_picking/pose_reference_frame', 'base_link')
object_request_service_name = rosparamOrDefault(
'/bin_picking/object_request_service', '/object_request')
rospy.wait_for_service(object_request_service_name)
self.requestObjectPose = rospy.ServiceProxy(
object_request_service_name, triple_s_util.srv.ObjectRequest)
self.controlGripper = rospy.ServiceProxy(
rosparamOrDefault('/bin_picking/gripper_service', '/control_rg2'),
onrobot_rg2.srv.ControlRG2)
self.approachPosePublisher = rospy.Publisher(
'/bin_picking/approach_pose',
geometry_msgs.msg.PoseStamped,
queue_size=10)
self.objectPosePublisher = rospy.Publisher(
'/bin_picking/object_pose',
geometry_msgs.msg.PoseStamped,
queue_size=10)
self.graspPosePublisher = rospy.Publisher(
'/bin_picking/grasp_pose',
geometry_msgs.msg.PoseStamped,
queue_size=10)
self.rodriguesPosePublisher = rospy.Publisher(
'/bin_picking/rodrigues_pose',
geometry_msgs.msg.PoseArray,
queue_size=10)
self.service = rospy.Service(
rosparamOrDefault('/bin_picking/pick_up_request_service',
'/pick_up_request'),
triple_s_util.srv.PickupRequest, self.onPickUpRequest)
def __init__(self):
self.service = rospy.Service(
rosparamOrDefault('/bin_picking/object_request_service',
'/object_request'),
triple_s_util.srv.ObjectRequest, self.onObjectRequest)
self.object_dimensions = rosparamOrDefault('/dope/dimensions', {})
self.object_meshes = rosparamOrDefault('/dope/meshes', {})
self.object_colors = rosparamOrDefault('/dope/draw_colors', {})
self.object_mesh_scale = rosparamOrDefault('/dope/mesh_scales', {})
self.min_x_object = rosparamOrDefault('/bin_picking/min_x_object',
-100)
self.max_x_object = rosparamOrDefault('/bin_picking/max_x_object', 100)
self.min_y_object = rosparamOrDefault('/bin_picking/min_y_object',
-100)
self.max_y_object = rosparamOrDefault('/bin_picking/max_y_object', 100)
self.min_z_object = rosparamOrDefault('/bin_picking/min_z_object',
-100)
self.max_z_object = rosparamOrDefault('/bin_picking/max_z_object', 100)
self.pose_publishers = {}
for name, dimension in self.object_dimensions.items():
self.pose_publishers[name] = rospy.Publisher(
rosparamOrDefault('/bin_picking/dope_pose_topic_prefix',
'/dope/pose_') + name,
geometry_msgs.msg.PoseStamped,
queue_size=10)
self.marker_publisher = rospy.Publisher(rosparamOrDefault(
'/bin_picking/dope_markers_topic', '/dope/markers'),
MarkerArray,
queue_size=10)
self.marker_id = 0
#!/usr/bin/env python
"""
Author: Niels de Boer
Date: 15-12-2020
Description: Continuously request objects
"""
import rospy
import random
from triple_s_util.srv import PickupRequest, PickupRequestRequest
from triple_s_util.bin_picking.util import rosparamOrDefault
rospy.init_node('continuous_random', anonymous=True)
objects = rosparamOrDefault('/dope/class_ids', {}).keys()
service_name = rosparamOrDefault('/bin_picking/pick_up_request_service',
'/pick_up_request')
rospy.wait_for_service(service_name)
service = rospy.ServiceProxy(service_name, PickupRequest)
while True:
request = PickupRequestRequest()
request.object_name = random.choice(objects)
print 'Requesting object of type: %s' % request.object_name
print service(request)
rospy.sleep(2)
def __init__(self):
self.dopeCollection = DopeCollection()
self.tf = tf.TransformListener()
self.forwardImage = ForwardImage(
input_camera_raw=rosparamOrDefault('/bin_picking/camera_raw',
'/d435_sim/camera_raw'),
input_camera_info=rosparamOrDefault('/bin_picking/camera_info',
'/d435_sim/camera_info'),
output_camera_raw=rosparamOrDefault('/bin_picking/dope_camera_raw',
'/dope/camera_raw'),
output_camera_info=rosparamOrDefault(
'/bin_picking/dope_camera_info', '/dope/camera_info'))
# Setup service
self.service = rospy.Service(
rosparamOrDefault('/bin_picking/object_request_service',
'/object_request'),
triple_s_util.srv.ObjectRequest, self.onObjectRequest)
if not rospy.has_param('/dope/class_ids'):
rospy.logerr(
'FindObject: /dope/class_ids is not loaded on the parameter server!'
)
sys.exit(1)
self.classIds = rospy.get_param('/dope/class_ids')
self.pose_reference_frame = rosparamOrDefault(
'/bin_picking/pose_reference_frame', 'base_link')
self.min_x_object = rosparamOrDefault('/bin_picking/min_x_object',
-100)
self.max_x_object = rosparamOrDefault('/bin_picking/max_x_object', 100)
self.min_y_object = rosparamOrDefault('/bin_picking/min_y_object',
-100)
self.max_y_object = rosparamOrDefault('/bin_picking/max_y_object', 100)
self.min_z_object = rosparamOrDefault('/bin_picking/min_z_object',
-100)
self.max_z_object = rosparamOrDefault('/bin_picking/max_z_object', 100)
rospy.loginfo('Done initializing find_object.py')
