def __init__(self):
self.object_publisher = rospy.Publisher(
'/art/object_detector/object_filtered',
InstancesArray,
queue_size=10,
latch=True)
self.objects = []
obj = ObjInstance()
obj.object_id = "profile_20_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.3
obj.pose.position.y = 0.3
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
obj = ObjInstance()
obj.object_id = "profile_21_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.75
obj.pose.position.y = 0.58
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
def __init__(self):
self.detected_objects_pub = rospy.Publisher("/art_object_detector/object", InstancesArray, queue_size=10)
while not rospy.is_shutdown():
instances = InstancesArray()
obj_in = ObjInstance()
obj_in.object_id = str('box')
obj_in.pose.position.x = 0.5
obj_in.pose.position.y = 0.5
obj_in.pose.position.z = 0
obj_in.pose.orientation.x = 0
obj_in.pose.orientation.y = 0
obj_in.pose.orientation.z = 0
obj_in.pose.orientation.w = 1.0
obj_in.bbox.dimensions = [0.05, 0.05, 0.1]
obj_in.bbox.type = SolidPrimitive.BOX
instances.header.frame_id = "/marker"
instances.instances.append(obj_in)
self.detected_objects_pub.publish(instances)
time.sleep(1)
pass
def get_pick_obj_from_feeder(obj_type):
assert isinstance(obj_type, str)
obj = ObjInstance()
obj.object_id = None
obj.object_type = obj_type
obj.pose = Pose()
return obj
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
# list of allowed object ids
# TODO load from param
if aid not in [4, 5, 3, 21, 26, 31]:
continue
if aid not in self.objects_cache:
# TODO AR code ID / object type assignment should be done somewhere...
# type "profile_20_60" is just for example (used in art_db/test_db.py)
if aid == 21:
object_type = self.art.get_object_type(
"profile_20_60_longer")
else:
object_type = self.art.get_object_type("profile_20_60")
if object_type is None:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {
'type': object_type.name,
'bb': object_type.bbox
}
# skip unknown objects
if self.objects_cache[aid] is None:
continue
obj_in = ObjInstance()
obj_in.object_id = str(aid)
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id,
self.objects_cache[aid]['bb'])
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
if len(data.markers) == 0:
rospy.logdebug("Empty")
else:
# print instances
self.detected_objects_pub.publish(instances)
def __init__(self):
self.object_publisher = rospy.Publisher(
'/art/object_detector/object_filtered',
InstancesArray,
queue_size=10,
latch=True)
self.srv_set_flag = rospy.Service('/art/object_detector/flag/set',
ObjectFlagSet, self.set_cb)
self.srv_clear_flag = rospy.Service('/art/object_detector/flag/clear',
ObjectFlagClear, self.clear_cb)
self.srv_clear_all_flags = rospy.Service(
'/art/object_detector/flag/clear_all', Empty, self.empty_cb)
self.forearm_cams = ("/l_forearm_cam_optical_frame",
"/r_forearm_cam_optical_frame")
self.srv_enable_forearm = rospy.Service(
'/art/object_detector/forearm/enable', Empty, self.empty_cb)
self.srv_disable_forearm = rospy.Service(
'/art/object_detector/forearm/disable', Empty, self.empty_cb)
self.srv_enable_detection = rospy.Service(
'/art/object_detector/all/enable', Empty, self.empty_cb)
self.srv_disable_detection = rospy.Service(
'/art/object_detector/all/disable', Empty, self.empty_cb)
self.objects = []
obj = ObjInstance()
obj.object_id = "profile_20_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.3
obj.pose.position.y = 0.3
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
obj = ObjInstance()
obj.object_id = "profile_21_60"
obj.object_type = "profile_20_60"
obj.pose = Pose()
obj.pose.position.x = 0.75
obj.pose.position.y = 0.58
obj.pose.position.z = 0
obj.pose.orientation.x = 0
obj.pose.orientation.y = 0
obj.pose.orientation.z = 0
obj.pose.orientation.w = 1
self.objects.append(obj)
def __init__(self, obj_id, frame_id, pos, rpy, noise):
self.object_publisher = rospy.Publisher('/art/object_detector/object',
InstancesArray,
queue_size=10,
latch=True)
self.frame_id = frame_id
self.pos = pos
self.noise = noise
self.obj = ObjInstance()
self.obj.object_id = obj_id
self.obj.object_type = "profile_20_60"
angles = list(rpy)
for idx in range(0, len(angles)):
angles[idx] = angles[idx] / 360.0 * 2 * pi
# TODO apply noise also to orientation
a2q(self.obj.pose.orientation,
transformations.quaternion_from_euler(*angles))
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
if aid not in self.objects_cache:
try:
resp = self.get_object_srv(obj_id=aid)
except rospy.ServiceException, e:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {'name': resp.name, 'model_url': resp.model_url, 'type': resp.type, 'bb': resp.bbox}
if self.objects_cache[aid] is None: continue
obj_in = ObjInstance()
obj_in.object_id = self.objects_cache[aid]['name']
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
angles = transformations.euler_from_quaternion([obj_in.pose.orientation.x,
obj_in.pose.orientation.y,
obj_in.pose.orientation.z,
obj_in.pose.orientation.w])
q = transformations.quaternion_from_euler(0, 0, angles[2])
obj_in.pose.orientation.x = q[0]
obj_in.pose.orientation.y = q[1]
obj_in.pose.orientation.z = q[2]
obj_in.pose.orientation.w = q[3]
obj_in.bbox = self.objects_cache[aid]['bb']
obj_in.pose.position.z = float(obj_in.bbox.dimensions[2]/2)
self.show_rviz_bb(obj_in, arcode.id)
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
def getRandomObject():
tmp = ObjInstance()
tmp.object_id = "object_" + str(random.randint(1,10000))
tmp.pose.position.x = random.uniform(0.5, 0.9)
tmp.pose.position.y = random.uniform(-0.8, 0.8)
tmp.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
tmp.pose.orientation.x = 0.0
tmp.pose.orientation.y = 0.0
tmp.pose.orientation.z = 0.0
tmp.pose.orientation.w = 1.0
tmp.bbox = SolidPrimitive()
tmp.bbox.type = SolidPrimitive.BOX
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.2)
return tmp
def getRandomObject():
tmp = ObjInstance()
tmp.object_id = "object_" + str(random.randint(1, 10000))
tmp.pose.position.x = random.uniform(0.5, 0.9)
tmp.pose.position.y = random.uniform(-0.8, 0.8)
tmp.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
tmp.pose.orientation.x = 0.0
tmp.pose.orientation.y = 0.0
tmp.pose.orientation.z = 0.0
tmp.pose.orientation.w = 1.0
tmp.bbox = SolidPrimitive()
tmp.bbox.type = SolidPrimitive.BOX
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.05)
tmp.bbox.dimensions.append(0.2)
return tmp
def __init__(self):
self.get_ready_srv = rospy.Service("arm/get_ready", Trigger,
self.get_ready_cb)
self.get_object_type_srv = rospy.ServiceProxy(
"/art/db/object_type/get", getObjectType)
self.get_pose_srv = rospy.ServiceProxy("/DobotServer/GetPose", GetPose)
self.move_to_srv = rospy.ServiceProxy("/DobotServer/SetPTPCmd",
SetPTPCmd)
self.set_suction_srv = rospy.ServiceProxy(
"/DobotServer/SetEndEffectorSuctionCup", SetEndEffectorSuctionCup)
self.get_suction_srv = rospy.ServiceProxy(
"/DobotServer/GetEndEffectorSuctionCup", GetEndEffectorSuctionCup)
self.set_cmd_timeout_srv = rospy.ServiceProxy(
"/DobotServer/SetCmdTimeout", SetCmdTimeout)
self.set_queued_cmd_clear_srv = rospy.ServiceProxy(
"/DobotServer/SetQueuedCmdClear", SetQueuedCmdClear)
self.set_queued_cmd_start_exec_srv = rospy.ServiceProxy(
"/DobotServer/SetQueuedCmdStartExec", SetQueuedCmdStartExec)
self.set_ent_effector_params_srv = rospy.ServiceProxy(
"/DobotServer/SetEndEffectorParams", SetEndEffectorParams)
self.set_ptp_joint_params_srv = rospy.ServiceProxy(
"/DobotServer/SetPTPJointParams", SetPTPJointParams)
self.set_ptp_coord_params_srv = rospy.ServiceProxy(
"/DobotServer/SetPTPCoordinateParams", SetPTPCoordinateParams)
self.set_ptp_jump_params_srv = rospy.ServiceProxy(
"/DobotServer/SetPTPJumpParams", SetPTPJumpParams)
self.set_ptp_common_params_srv = rospy.ServiceProxy(
"/DobotServer/SetPTPCommonParams", SetPTPCommonParams)
self._action_name = "arm/pp"
self._as = actionlib.SimpleActionServer(self._action_name,
PickPlaceAction,
self.pick_place_cb, True)
self.move_to_sub = rospy.Subscriber("arm/move_to",
PoseStamped,
self.move_to_cb,
queue_size=100)
self.object_sub = rospy.Subscriber(
"/art/object_detector/object_filtered",
InstancesArray,
self.objects_cb,
queue_size=1)
self.tf_broadcaster = TransformBroadcaster()
self.tf_listener = TransformListener()
self._dobot_pose = Pose()
self._objects = InstancesArray()
self._grasped_object = ObjInstance()
self._init_dobot()
rospy.loginfo("Dobot ready")
def timer_cb(self, event):
ia = InstancesArray()
ia.header.frame_id = self.target_frame
ia.header.stamp = rospy.Time.now()
objects_to_prune = []
now = rospy.Time.now()
for k, v in self.objects.iteritems():
if (now - v["pose"].header.stamp) > self.max_age:
objects_to_prune.append(k)
ia.lost_objects.append(k)
continue
if v["cnt"] < self.min_cnt:
continue
obj = ObjInstance()
obj.pose = v["pose"].pose
obj.pose.orientation = self.normalize(obj.pose.orientation)
obj.object_id = k
obj.object_type = v["object_type"]
ia.instances.append(obj)
if v["cnt"] == self.min_cnt:
ia.new_objects.append(k)
# TODO also publish TF for each object???
self.pub.publish(ia)
for k in objects_to_prune:
rospy.loginfo("Object " + k + " no longer visible")
del self.objects[k]
if len(objects_to_prune) > 0:
rospy.loginfo("Pruned " + str(len(objects_to_prune)) + " objects")
def place_object(self, place_pose):
"""
Args:
place_pose:
@type place_pose: PoseStamped
Returns:
"""
print self._grasped_object.object_type
print self.get_object_type(self._grasped_object.object_type)
obj_type = self.get_object_type(self._grasped_object.object_type)
place_pose.pose.position.z = obj_type.bbox.dimensions[
obj_type.bbox.BOX_Z]
# self.get_ready_for_pick_place()
transformed_pose = self.tf_listener.transformPose(
"/base_link", place_pose)
try:
pp = deepcopy(transformed_pose)
pp.pose.position.z = max(pp.pose.position.z + 0.05, 0.15)
self.move_to_pose(pp, 1)
self.wait_for_final_pose(pp.pose)
self.move_to_pose(transformed_pose)
self.wait_for_final_pose(transformed_pose.pose)
self.set_suction(False)
transformed_pose.pose.position.z += 0.03
self.move_to_pose(transformed_pose)
self.wait_for_final_pose(transformed_pose.pose)
self._grasped_object = ObjInstance()
self.grasped_object_pub.publish(self._grasped_object)
res = PickPlaceResult()
res.result = res.SUCCESS
return res
except TimeoutException:
# failed to place object
self.clear_all_alarm_state.call()
res = PickPlaceResult()
res.result = res.FAILURE
res.message = "Failed to place the object"
return res
def main():
pub = rospy.Publisher("/art/object_detector/object_filtered",
InstancesArray,
queue_size=10)
pub_point_left = rospy.Publisher("/art/user/pointing_left",
PoseStamped,
queue_size=10)
pub_point_right = rospy.Publisher("/art/user/pointing_right",
PoseStamped,
queue_size=10)
arr = InstancesArray()
arr.header.frame_id = "marker"
obj = ObjInstance()
obj.object_id = "3"
obj.object_type = "profile_20_60"
obj.pose.position.x = 0.5
obj.pose.position.y = 0.5
obj.pose.position.z = 0.08
obj.pose.orientation = yaw2orientation(0)
arr.instances.append(obj)
obj2 = ObjInstance()
obj2.object_id = "4"
obj2.object_type = "profile_20_60"
obj2.pose.position.x = 0.7
obj2.pose.position.y = 0.2
obj2.pose.position.z = 0.08
obj2.pose.orientation = yaw2orientation(45 / 2)
arr.instances.append(obj2)
obj3 = ObjInstance()
obj3.object_id = "even_another_object"
obj3.object_type = "profile_20_60"
obj3.pose.position.x = 0.9
obj3.pose.position.y = 0.3
obj3.pose.position.z = 0.08
obj3.pose.orientation = yaw2orientation(45)
arr.instances.append(obj3)
arr.new_objects.append(obj.object_id)
arr.new_objects.append(obj2.object_id)
arr.new_objects.append(obj3.object_id)
rospy.sleep(1.0)
pub.publish(arr)
rospy.sleep(1.0)
arr.new_objects = []
while (not rospy.is_shutdown()):
arr.header.stamp = rospy.Time.now()
pub.publish(arr)
rospy.sleep(1.0)
def get_pick_obj_from_feeder(instruction):
obj = ObjInstance()
obj.object_id = None
obj.object_type = instruction.object[0]
obj.pose = Pose()
return obj
def main():
pub = rospy.Publisher("/art/object_detector/object_filtered", InstancesArray, queue_size=10)
pub_point_left = rospy.Publisher("/art/user/pointing_left", PoseStamped, queue_size=10)
pub_point_right = rospy.Publisher("/art/user/pointing_right", PoseStamped, queue_size=10)
arr = InstancesArray()
arr.header.frame_id = "marker"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.object_type = "object"
obj.pose.position.x = 0.5
obj.pose.position.y = 0.5
obj.pose.position.z = 0.0
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
obj2 = ObjInstance()
obj2.object_id = "another_object"
obj2.object_type = "object"
obj2.pose.position.x = 0.7
obj2.pose.position.y = 0.3
obj2.pose.position.z = 0.0
obj2.pose.orientation.x = 0.0
obj2.pose.orientation.y = 0.0
obj2.pose.orientation.z = 0.0
obj2.pose.orientation.w = 1.0
obj2.bbox = SolidPrimitive()
obj2.bbox.type = SolidPrimitive.BOX
obj2.bbox.dimensions.append(0.05)
obj2.bbox.dimensions.append(0.05)
obj2.bbox.dimensions.append(0.2)
arr.instances.append(obj2)
arr.new_objects.append(obj.object_id)
arr.new_objects.append(obj2.object_id)
ps = PoseStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = "marker"
ps.pose.position.x = 0.0
ps.pose.position.y = 0.5
ps.pose.position.z = 0
ps.pose.orientation.x = 0.0
ps.pose.orientation.y = 0.0
ps.pose.orientation.z = 0.0
ps.pose.orientation.w = 1.0
psr = PoseStamped()
psr.header.stamp = rospy.Time.now()
psr.header.frame_id = "marker"
psr.pose.position.x = 0.0
psr.pose.position.y = 0.3
psr.pose.position.z = 0
psr.pose.orientation.x = 0.0
psr.pose.orientation.y = 0.0
psr.pose.orientation.z = 0.0
psr.pose.orientation.w = 1.0
noise = 0.0001
rospy.sleep(2.0)
pub.publish(arr)
rospy.sleep(1.0)
arr.new_objects = []
while(not rospy.is_shutdown()):
pub.publish(arr)
rospy.sleep(1.0)
while(not rospy.is_shutdown()):
if psr.pose.position.x < 0.8:
psr.pose.position.x += 0.003
else:
psr.pose.position.x = 0
psr.pose.position.y = 0.3
pub_point_right.publish(psr)
if ps.pose.position.x < 0.8:
ps.pose.position.x += 0.002
else:
ps.pose.position.x = 0
ps.pose.position.y = 0.5
if isclose(ps.pose.position.x, 0.5):
for i in range(0, 300):
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
if isclose(ps.pose.position.x, 0.6):
for i in range(0, 300):
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
pub.publish(arr)
pub_point_left.publish(ps)
rospy.sleep(0.01)
def main():
pub = rospy.Publisher("/art_object_detector/object_filtered", InstancesArray)
client = actionlib.SimpleActionClient('/pr2_pick_place_left/pp', art_msgs.msg.pickplaceAction)
client.wait_for_server()
arr = InstancesArray()
arr.header.frame_id = "base_footprint"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.pose.position.x = random.uniform(0.4, 0.7)
obj.pose.position.y = random.uniform(-0.2, 0.5)
obj.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
arr.instances.append(getRandomObject())
arr.instances.append(getRandomObject())
pub.publish(arr)
rospy.sleep(2.0)
goal = art_msgs.msg.pickplaceGoal()
goal.id = "my_object"
goal.operation = goal.PICK_AND_PLACE
goal.z_axis_angle_increment = (2*3.14)/360*90
goal.keep_orientation = False
goal.place_pose = PoseStamped()
goal.place_pose.header.frame_id = "base_footprint"
goal.place_pose.header.stamp = rospy.Time.now()
goal.place_pose.pose.position.x = random.uniform(0.4, 0.7)
goal.place_pose.pose.position.y = random.uniform(-0.2, 0.5)
goal.place_pose.pose.position.z = 0.74 + 0.1
goal.place_pose.pose.orientation.x = 0.0
goal.place_pose.pose.orientation.y = 0.0
goal.place_pose.pose.orientation.z = 0.0
goal.place_pose.pose.orientation.w = 1.0
rospy.loginfo('sending goal')
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo('got result')
print client.get_result()
print "status: " + client.get_goal_status_text()
print "state: " + str(client.get_state())
def inst(self, table_size, ground_objects_on_table=False, ground_bb_axis=SolidPrimitive.BOX_Z,
yaw_only_on_table=False):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type.name
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.on_table = 0 < inst.pose.position.x < table_size[0] and 0 < inst.pose.position.y < table_size[1]
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
q_arr = transformations.quaternion_from_euler(*cur_rpy)
# ground objects that are really sitting on the table (exclude those in the air)
if inst.on_table and ground_objects_on_table and np.average(
pz, weights=w) < self.object_type.bbox.dimensions[ground_bb_axis] / 2.0 + 0.1:
# TODO consider orientation!
inst.pose.position.z = self.object_type.bbox.dimensions[ground_bb_axis] / 2.0
if yaw_only_on_table:
# TODO figure out which axis should be kept
# ...like this it only works for some objects (containers)
q_arr[0] = 0.0
q_arr[1] = 0.0
q_arr = transformations.unit_vector(q_arr)
else:
inst.pose.position.z = np.average(pz, weights=w)
a2q(inst.pose.orientation, q_arr)
for (key, value) in self.flags.iteritems():
kv = KeyValue()
kv.key = key
kv.value = value
inst.flags.append(kv)
return inst
def ar_code_cb(self, data):
rospy.logdebug("New arcodes arrived:")
instances = InstancesArray()
id = 0
for arcode in data.markers:
aid = int(arcode.id)
# list of allowed object ids
# TODO load from param
if aid not in [
50, 51, 52, 53, 54, 55, 56, 57, 60, 61, 62, 63, 1001, 1002,
1003, 1004, 1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012,
1013, 1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022,
1023, 1024, 1025, 2001, 2002, 2003, 2004, 2005, 2006, 2007,
2008, 3001, 3002, 3003, 3004
]:
continue
if aid not in self.objects_cache:
# TODO AR code ID / object type assignment should be done somewhere...
# type "profile_20_60" is just for example (used in art_db/test_db.py)
object_type = None
if aid in [
50, 51, 52, 53, 54, 55, 56, 57, 1001, 1002, 1003, 1004,
1005, 1006, 1007, 1008, 1009, 1010, 1011, 1012, 1013,
1014, 1015, 1016, 1017, 1018, 1019, 1020, 1021, 1022,
1023, 1024, 1025
]:
object_type = self.art.get_object_type("Spojka")
elif aid in [
60, 61, 62, 63, 2001, 2002, 2003, 2004, 2005, 2006,
2007, 2008
]:
object_type = self.art.get_object_type("Kratka_noha")
elif aid in [60, 61, 62, 63, 3001, 3002, 3003, 3004]:
object_type = self.art.get_object_type("Dlouha_noha")
if object_type is None:
# error or unknown object - let's ignore it
self.objects_cache[aid] = None
continue
self.objects_cache[aid] = {
'type': object_type.name,
'bb': object_type.bbox
}
# skip unknown objects
if self.objects_cache[aid] is None:
continue
obj_in = ObjInstance()
obj_in.object_id = str(aid)
obj_in.object_type = self.objects_cache[aid]['type']
obj_in.pose = arcode.pose.pose
self.show_rviz_bb(arcode.header.frame_id, obj_in, arcode.id,
self.objects_cache[aid]['bb'])
instances.header.stamp = arcode.header.stamp
instances.header.frame_id = arcode.header.frame_id
instances.instances.append(obj_in)
++id
if len(data.markers) == 0:
rospy.logdebug("Empty")
else:
# print instances
self.detected_objects_pub.publish(instances)
def inst(self):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.pose.position.z = np.average(pz, weights=w)
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
# TODO hysteresis!
# TODO is there a smarter way how to do it?
# "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees
# yaw (in table coordinates) may stay as it is
for i in range(0, 2):
if cur_rpy[i] < 0.0:
cur_rpy[i] = 2 * pi + cur_rpy[i]
if cur_rpy[i] >= 7 * pi / 4 or cur_rpy[i] < pi / 4:
cur_rpy[i] = 0.0
elif cur_rpy[i] >= pi / 4 and cur_rpy[i] < 3 * pi / 4:
cur_rpy[i] = pi / 2
elif cur_rpy[i] >= 3 * pi / 4 and cur_rpy[i] < 5 * pi / 4:
cur_rpy[i] = pi
elif cur_rpy[i] >= 5 * pi / 4 and cur_rpy[i] < 7 * pi / 4:
cur_rpy[i] = 3 * pi / 2
a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy))
return inst
def inst(self):
inst = ObjInstance()
inst.object_id = self.object_id
inst.object_type = self.object_type
w = []
px = []
py = []
pz = []
r = []
p = []
y = []
for frame_id, val in self.meas.iteritems():
if len(val) < 2:
continue
for idx in range(0, len(val)):
v = val[idx]
# distance normalized to 0, 1
d = (v[0] - self.min_dist) / (self.max_dist - self.min_dist)
# weight based on distance from object to sensor
w_dist = (1.0 - d) ** 2 # (0, 1)
# newer detections are more interesting
w_age = (float(idx) / (len(val) - 1)) / 2 + 0.5 # (0.5, 1)
w.append(w_dist * w_age)
px.append(v[1].pose.position.x)
py.append(v[1].pose.position.y)
pz.append(v[1].pose.position.z)
rpy = transformations.euler_from_quaternion(q2a(v[1].pose.orientation))
r.append([cos(rpy[0]), sin(rpy[0])])
p.append([cos(rpy[1]), sin(rpy[1])])
y.append([cos(rpy[2]), sin(rpy[2])])
if len(w) < self.min_meas_cnt:
return None
inst.pose.position.x = np.average(px, weights=w)
inst.pose.position.y = np.average(py, weights=w)
inst.pose.position.z = np.average(pz, weights=w)
# TODO read table size from param
inst.on_table = 0 < inst.pose.position.x < 1.5 and 0 < inst.pose.position.x < 0.7
ar = np.average(r, axis=0, weights=w)
ap = np.average(p, axis=0, weights=w)
ay = np.average(y, axis=0, weights=w)
fr = atan2(ar[1], ar[0])
fp = atan2(ap[1], ap[0])
fy = atan2(ay[1], ay[0])
cur_rpy = [fr, fp, fy]
a2q(inst.pose.orientation, transformations.quaternion_from_euler(*cur_rpy))
# TODO "fix" roll and pitch so they are only 0, 90, 180 or 270 degrees
# yaw (in table coordinates) may stay as it is
for (key, value) in self.flags.iteritems():
kv = KeyValue()
kv.key = key
kv.value = value
inst.flags.append(kv)
return inst
def main():
pub = rospy.Publisher("/art/object_detector/object_filtered",
InstancesArray,
queue_size=10)
pub_point_left = rospy.Publisher("/art/user/pointing_left",
PoseStamped,
queue_size=10)
pub_point_right = rospy.Publisher("/art/user/pointing_right",
PoseStamped,
queue_size=10)
br = tf.TransformBroadcaster()
arr = InstancesArray()
arr.header.frame_id = "marker"
obj = ObjInstance()
obj.object_id = "3"
obj.object_type = "profile_20_60"
obj.pose.position.x = 0.5
obj.pose.position.y = 0.5
obj.pose.position.z = 0.025
obj.pose.orientation = yaw2orientation(90, 0, 0)
arr.instances.append(obj)
obj2 = ObjInstance()
obj2.object_id = "4"
obj2.object_type = "profile_20_60"
obj2.pose.position.x = 0.7
obj2.pose.position.y = 0.2
obj2.pose.position.z = 0.08
obj2.pose.orientation = yaw2orientation(0, 0, 45 / 2)
arr.instances.append(obj2)
obj3 = ObjInstance()
obj3.object_id = "even_another_object"
obj3.object_type = "profile_20_60"
obj3.pose.position.x = 0.9
obj3.pose.position.y = 0.3
obj3.pose.position.z = 0.025
obj3.pose.orientation = yaw2orientation(90, 90, 45)
arr.instances.append(obj3)
arr.new_objects.append(obj.object_id)
arr.new_objects.append(obj2.object_id)
arr.new_objects.append(obj3.object_id)
rospy.sleep(1.0)
pub.publish(arr)
rospy.sleep(1.0)
arr.new_objects = []
while not rospy.is_shutdown():
arr.header.stamp = rospy.Time.now()
pub.publish(arr)
for obj in arr.instances:
br.sendTransform((obj.pose.position.x, obj.pose.position.y,
obj.pose.position.z),
(obj.pose.orientation.x, obj.pose.orientation.y,
obj.pose.orientation.z, obj.pose.orientation.w),
arr.header.stamp, "object_id_" + obj.object_id,
"marker")
rospy.sleep(0.25)
def main():
pub = rospy.Publisher("/art_object_detector/object_filtered",
InstancesArray)
client = actionlib.SimpleActionClient('/pr2_pick_place_left/pp',
art_msgs.msg.pickplaceAction)
client.wait_for_server()
arr = InstancesArray()
arr.header.frame_id = "base_footprint"
arr.header.stamp = rospy.Time.now()
obj = ObjInstance()
obj.object_id = "my_object"
obj.pose.position.x = random.uniform(0.4, 0.7)
obj.pose.position.y = random.uniform(-0.2, 0.5)
obj.pose.position.z = 0.74 + 0.1 # vyska stolu + pulka kosticky
obj.pose.orientation.x = 0.0
obj.pose.orientation.y = 0.0
obj.pose.orientation.z = 0.0
obj.pose.orientation.w = 1.0
obj.bbox = SolidPrimitive()
obj.bbox.type = SolidPrimitive.BOX
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.05)
obj.bbox.dimensions.append(0.2)
arr.instances.append(obj)
arr.instances.append(getRandomObject())
arr.instances.append(getRandomObject())
pub.publish(arr)
rospy.sleep(2.0)
goal = art_msgs.msg.pickplaceGoal()
goal.id = "my_object"
goal.operation = goal.PICK_AND_PLACE
goal.z_axis_angle_increment = (2 * 3.14) / 360 * 90
goal.keep_orientation = False
goal.place_pose = PoseStamped()
goal.place_pose.header.frame_id = "base_footprint"
goal.place_pose.header.stamp = rospy.Time.now()
goal.place_pose.pose.position.x = random.uniform(0.4, 0.7)
goal.place_pose.pose.position.y = random.uniform(-0.2, 0.5)
goal.place_pose.pose.position.z = 0.74 + 0.1
goal.place_pose.pose.orientation.x = 0.0
goal.place_pose.pose.orientation.y = 0.0
goal.place_pose.pose.orientation.z = 0.0
goal.place_pose.pose.orientation.w = 1.0
rospy.loginfo('sending goal')
client.send_goal(goal)
client.wait_for_result()
rospy.loginfo('got result')
print client.get_result()
print "status: " + client.get_goal_status_text()
print "state: " + str(client.get_state())
