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)