def lookat_callback(data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
if (abs(data.data[0])<=1) and (abs(data.data[1])<=1):
x = data.data[0]
y = data.data[1]
neck.look_xy(x, y, 5)
else:
rospy.WARN("Inserire dei valori compresi fra -1 ed 1...")
def handle_recognize(req):
rsp = recognizeResponse()
bridge = CvBridge()
try:
# Convert rosmsg to cv image
# np_array = np.fromstring(req.img_in.data, np.uint8)
# image = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR)
cv_image = bridge.imgmsg_to_cv2(req.img_in, "bgr8")
(rows, cols, channels) = cv_image.shape
if cols != 640 or rows != 480:
rospy.WARN('Can not get image.')
return rsp
else:
result = ps.process(cv_image)
ro_msg = recognized_objects()
for i in result:
bbox = i[0]
score = i[1]
class_name = i[2]
cv2.rectangle(cv_image, (int(bbox[0]), int(bbox[1])),
(int(bbox[2]), int(bbox[3])), (255, 0, 255), 2)
cv2.putText(cv_image, '{:s} {:.3f}'.format(class_name, score),
(int(bbox[0]), int(bbox[1]) - 2),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
label_str_msg = String()
label_str_msg.data = class_name
bbox_array_msg = UInt16MultiArray()
for j in bbox:
bbox_array_msg.data.append(j)
ro_msg.labels.append(label_str_msg)
ro_msg.bbox_arrays.append(bbox_array_msg)
ro_msg.header = req.img_in.header
rsp.img_out = bridge.cv2_to_imgmsg(cv_image, "bgr8")
rsp.obj_info = ro_msg
return rsp
except CvBridgeError as e:
print(e)
return rsp
def gripper_client(finger_positions, prefix):
"""Send a gripper goal to the action server."""
action_address = '/' + prefix + 'driver/fingers_action/finger_positions'
client = actionlib.SimpleActionClient(
action_address, kinova_msgs.msg.SetFingersPositionAction)
client.wait_for_server()
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float(finger_positions[0])
goal.fingers.finger2 = float(finger_positions[1])
goal.fingers.finger3 = float(finger_positions[2])
client.send_goal(goal)
if client.wait_for_result(rospy.Duration(50.0)):
return client.get_result()
else:
client.cancel_all_goals()
rospy.WARN(' the gripper action timed-out')
return None
def gripper_client(self, finger_positions):
"""Send a gripper goal to the action server."""
self.client.wait_for_server()
self.goal.fingers.finger1 = float(finger_positions[0])
self.goal.fingers.finger2 = float(finger_positions[1])
# The MICO arm has only two fingers, but the same action definition is used
if len(finger_positions) < 3:
self.goal.fingers.finger3 = 0.0
else:
self.goal.fingers.finger3 = float(finger_positions[2])
self.client.send_goal(self.goal)
if self.client.wait_for_result(rospy.Duration(5.0)):
return self.client.get_result()
else:
self.client.cancel_all_goals()
rospy.WARN(' the gripper action timed-out')
return None
def gripper_client(finger_positions):
"""Send a gripper goal to the action server."""
action_address = '/' + prefix + 'driver/fingers_action/finger_positions'
action_address = '/j2n6s300_driver/fingers_action/finger_positions'
client = actionlib.SimpleActionClient(
action_address, kinova_msgs.msg.SetFingersPositionAction)
client.wait_for_server()
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float(finger_positions[0])
goal.fingers.finger2 = float(finger_positions[1])
# The MICO arm has only two fingers, but the same action definition is used
if len(finger_positions) < 3:
goal.fingers.finger3 = 0.0
else:
goal.fingers.finger3 = float(finger_positions[2])
client.send_goal(goal)
if client.wait_for_result(rospy.Duration(5.0)):
return client.get_result()
else:
client.cancel_all_goals()
rospy.WARN(' the gripper action timed-out')
return None
def set_finger_positions(finger_positions):
"""Send a gripper goal to the action server."""
global gripper_client
global finger_maxTurn
finger_positions[0] = min(finger_maxTurn, finger_positions[0])
finger_positions[1] = min(finger_maxTurn, finger_positions[1])
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float(finger_positions[0])
goal.fingers.finger2 = float(finger_positions[1])
# The MICO arm has only two fingers, but the same action definition is used
if len(finger_positions) < 3:
goal.fingers.finger3 = 0.0
else:
goal.fingers.finger3 = float(finger_positions[2])
gripper_client.send_goal(goal)
if gripper_client.wait_for_result(rospy.Duration(5.0)):
return gripper_client.get_result()
else:
gripper_client.cancel_all_goals()
rospy.WARN(' the gripper action timed-out')
return None
(rtclient,cur_client, 'h', run_event, joint))
move_t.start()
#---------------------------------------------------------------------------
while not rospy.is_shutdown():
"generate one time message"
otm = tform.merge(joint._payload)
print otm
#---------------------------------------------------------------------------
try:
"UDP send launch"
motorsock.sendto(otm, (ip[dev_name], port[dev_name]))
except socket.error as error:
if error.errno == errno.ENETUNREACH:
rospy.WARN("connection to mbed lost.")
else:
raise
if dev_name == 'wheel':
tform.make_message(pub_msg, 2, dev_name, 2, joint._payload_w)
pub.publish(pub_msg)
#---------------------------------------------------------------------------
rate.sleep()
if dev_name == 'wheel':
move_t.join()
__version = "1.1.0"
"""Send a gripper goal to the action server."""
client.wait_for_server()
finger_goal.fingers.finger1 = 0.0
finger_goal.fingers.finger2 = 0.0
finger_goal.fingers.finger3 = 0.0
client.send_goal(finger_goal)
if client.wait_for_result(rospy.Duration(5.0)):
finger_result = client.get_result()
mode = 1
else:
client.cancel_all_goals()
rospy.WARN(' the gripper action timed-out')
if (mode == 1):
# Find Position Vector End-Effector(e) to Goal(g) wrt Base Link Frame(b).
x_b = T_base2goal.transform.translation.x - T_base2end.transform.translation.x
y_b = T_base2goal.transform.translation.y - T_base2end.transform.translation.y
z_b = T_base2goal.transform.translation.z - T_base2end.transform.translation.z
P_e2g_b = [x_b, y_b, z_b]
# Find Quaternion(Q) and XYZ Euler(E) Rotation between End-effector(e) to Goal(g)
# Quaternion
Q_eg = [T_end2goal.transform.rotation.x, T_end2goal.transform.rotation.y, T_end2goal.transform.rotation.z, T_end2goal.transform.rotation.w]
# Euler XYZ
E_e2g = tf_conversions.transformations.euler_from_quaternion(Q_eg)
