def transformGrasp(graspConfigs): rate = rospy.Rate(10) tf_listener = tf.TransformListener(rospy.Duration(1)) br = tf.TransformBroadcaster() tf_base_camera = TransformStamped() # tf::StampedTransform tf_base_odom; qs = [] ts = [] t_pres = [] for i, graspConfig in enumerate(graspConfigs): approach_x = np.array([graspConfig.approach.x, graspConfig.approach.y, graspConfig.approach.z, 0]) binormal_y = np.array([graspConfig.binormal.x, graspConfig.binormal.y, graspConfig.binormal.z, 0]) axis_z = np.array([graspConfig.axis.x, graspConfig.axis.y, graspConfig.axis.z, 0]) padding = np.array([0,0,0,1]) rotation_matrix = np.array([approach_x, binormal_y, axis_z, padding]).T q = quaternion_from_matrix(rotation_matrix) t = np.array([graspConfig.top.x, graspConfig.top.y , graspConfig.top.z ]) qs.append(q) ts.append(t) print("Boardcast raw grasps in camera_rgb_optical_frame...") count_down_clock(3) while not rospy.is_shutdown(): for i in range(len(qs)): br.sendTransform(ts[i], qs[i], rospy.Time.now(), "grasp_"+str(i), "camera_rgb_optical_frame") rate.sleep()
def cloud_callback(msg): print("Send cloud data to remote") count_down_clock(4) s = requests.Session() payload = pickle.dumps({'data' : msg}) url = 'http://104.196.255.235:8000' headers = {'Content-Type': 'application/octet-stream'} response = s.post(url, headers=headers, data=payload) response_payload = response.content grasps = pickle.loads(response_payload) for grasp in grasps: print('################################') print(grasp) print('################################') testGrasp_point = transformGrasp(grasps)
def send_request(msg): print("Starting request node... ") print("Subscribing to /camera/depth_registered/points... ") print("Send cloud data to remote") count_down_clock(4) s = requests.Session() payload = pickle.dumps({'data': msg}) url = 'http://104.196.255.235:8000' headers = {'Content-Type': 'application/octet-stream'} response = s.post(url, headers=headers, data=payload) response_payload = response.content grasps = pickle.loads(response_payload) for grasp in grasps: print('################################') print(grasp) print('################################') testGrasp_point = transformGrasp(grasps) print("Subscribed to /camera/depth_registered/points...")
offset_grasp = -0.05 t_grasp = t + new_matrix[0:3, 2] * offset_grasp ts_grasp.append(t_grasp) q = quaternion_from_matrix(new_matrix) br = tf.TransformBroadcaster() rate = rospy.Rate(10) print("Find {0} grasps!".format(len(qs))) rospy.set_param('get_grasp_camera', 1) rospy.set_param('broad_grasp_base', 1) rospy.set_param('good_grasps', len(qs)) count_down_clock(3) while not rospy.is_shutdown(): if rospy.get_param('executed') rospy.set_param('executed', 0) break for i in range(len(qs)): br.sendTransform(ts_pre[i], qs[i], rospy.Time.now(), "sample_pregrasp_"+str(i), "base") br.sendTransform(ts_grasp[i], qs[i], rospy.Time.now(),