def rgb_callback(self, imgmessage): rgb_bridge = CvBridge() self.rgb_image = rgb_bridge.imgmsg_to_cv2(imgmessage, "bgr8") detect = ColorDetection(self.rgb_image) msg_to_send = Floats() msg_to_send.data = detect.image_uv(2) mypub = rospy.Publisher("uvtopic", Floats, queue_size=1) mypub.publish(msg_to_send)
def cb(msg): x = Floats() x.data.append(180 + (math.degrees(msg.position[0]))) x.data.append(180 + (math.degrees(msg.position[1]))) x.data.append(180 + (math.degrees(msg.position[2]))) x.data.append(180 + (math.degrees(msg.position[3]))) x.data.append(180 + (math.degrees(msg.position[4]))) x.data.append(180 + (math.degrees(msg.position[5]))) #rospy.sleep(1/2) pub.publish(x)
def cb(msg): x=Floats() x.data.append((math.degrees(msg.position[0]))) x.data.append((math.degrees(msg.position[1]))) x.data.append((math.degrees(msg.position[2]))) x.data.append((math.degrees(msg.position[3]))) x.data.append((math.degrees(msg.position[4]))) x.data.append((math.degrees(msg.position[5]))) ''' joints_pub.data.push_back((angles::to_degrees(joint_position_command_[0]))); joints_pub.data.push_back((angles::to_degrees(joint_position_command_[1]))); joints_pub.data.push_back((angles::to_degrees(joint_position_command_[2]))); joints_pub.data.push_back((angles::to_degrees(joint_position_command_[3]))); joints_pub.data.push_back((angles::to_degrees(joint_position_command_[4]))); joints_pub.data.push_back(+(angles::to_degrees(joint_position_command_[5]))); ''' pub.publish(x)
def getDetections(self, req): print('Request for detection received!! Waiting for detection updates...') # To get the most recent detections , we reset all the previous poses and labels self.poses = None self.cls_ids = None #self.panda_binImg = None #Reset this as well in order to get the binImg with latest panda-config in scene if rospy.has_param('remove_panda'): # This param helps other nodes, signal this server whether to remove the scene robot or not. self.remove_panda_dpt = rospy.get_param('remove_panda') if rospy.has_param('pvn3d_cam'): self.cam = rospy.get_param('pvn3d_cam') # Set the cam, we're getting detections from. self.cam_frame = rospy.get_param('pvn3d_cam_frame') # Set camFrame, to publish the poses in. # NOTE: In the final tutorial, we continuously switch between the camera fixed on robot and the camera looking at the scene & robot # If the testing scenario has one camera only, this param shouldn't exist and the img topics should be the default ones in class def. image_sub = message_filters.Subscriber(str(self.cam)+'/color/image_raw', Image, queue_size=1) depth_sub = message_filters.Subscriber(str(self.cam)+'/depth/image_raw', Image, queue_size=1) self.ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 10) self.ts.registerCallback(self.ros2torch_CB) rospy.sleep(1) # Finally set this, in order to get the TSCallback working. self.request = True while True: ## Wait for the time synchronizer to completely go through the ros2torch callback - This is roughly equal to the inference speed if self.cls_ids is not None: #Whenever we get any detection updates... self.request = False ## Reset the detection request so that the callback doesn't go in auto-detect mode. self.panda_binImg = None #Reset this as well in order to get the binImg with latest panda-config in scene poses_msg = [] for pose in self.poses: rot = R.from_dcm(pose[0:3,0:3]) quat = rot.as_quat() poses_msg.append( Pose(position = Point(x=pose[0,3],y=pose[1,3],z=pose[2,3]), orientation = Quaternion(x=quat[0],y=quat[1],z=quat[2],w=quat[3]) ) ) pose_arr_msg = PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.cam_frame), poses = poses_msg) return getPoses_resp(pose_arr_msg , Floats( self.cls_ids.tolist() ))
def get_grasps_handle(self, req): print('compute_grasps request recieved...') self.get_detections( ) # Request an update on detections everytime, we recieve a computeGrasp request. self.arrange_poses_euc_dist( ) #Arrange so that robot always picks up the closest object first # The cam frame of the camera used for detections i.e., There are two cameras being used for the demo if rospy.has_param('pvn3d_cam_frame'): self.cam_frame = rospy.get_param('pvn3d_cam_frame') try: tf_stamped = tf_buffer.lookup_transform('world', self.cam_frame, rospy.Time(0)) poses_in_world = self.transform_poses(self.poses, tf_stamped) grasp_msgs = [] grasp_labels = [] grasp_offs_X = [] grasp_offs_Y = [] grasp_offs_yaw = [] if self.labels is not None: for i, lb in enumerate(self.labels): preDefined_grasps = np.loadtxt( './datasets/openDR/dataset_config/preDefined_grasps/' + str(int(lb)) + '.txt') print('Loaded ' + str(preDefined_grasps.shape[0]) + ' grasps for ' + self.cls_dict[lb]) for j in range(preDefined_grasps.shape[0]): gr_rot = R.from_euler('zyx', preDefined_grasps[j, 3:6]).as_dcm() gr_pos = preDefined_grasps[j, 0:3].reshape(3, 1) #obj_pos = np.array([poses_in_world[i].position.x, poses_in_world[i].position.y, poses_in_world[i].position.z]) #obj_rot = R.from_quat(np.array([poses_in_world[i].orientation.x, poses_in_world[i].orientation.y, poses_in_world[i].orientation.z, poses_in_world[i].orientation.w]) ).as_dcm() grasp_in_obj = np.vstack((np.hstack( (gr_rot, gr_pos)), np.array([0, 0, 0, 1]))) #obj_in_world = np.hstack(( obj_rot, obj_pos )) obj_in_world = poses_in_world[i] grasp_in_world = np.matmul(obj_in_world, grasp_in_obj) grasp_msgs.append( ros_numpy.msgify(Pose, grasp_in_world)) #grasp_msgs.append(ros_numpy.msgify(Pose, self.poses )) grasp_labels.append( lb ) ## Labeling every grasp with its respective class label. grasp_offs_X.append(preDefined_grasps[j, 6]) grasp_offs_Y.append(preDefined_grasps[j, 7]) grasp_offs_yaw.append(preDefined_grasps[j, 8]) print(grasp_labels) self.grasp_pub.publish( PoseArray(header=Header(stamp=rospy.Time.now(), frame_id='world'), poses=grasp_msgs)) return PoseArray( header=Header(stamp=rospy.Time.now(), frame_id='world'), poses=grasp_msgs), Floats(grasp_labels), Floats( grasp_offs_X), Floats(grasp_offs_Y), Floats( grasp_offs_yaw) except Exception as inst: exc_type, exc_obj, exc_tb = sys.exc_info() print('exception: ' + str(inst) + ' in ' + str(exc_tb.tb_lineno))
if robot is at a different state from the what interface indicates this node will make both synchronized the node should work for few seconds and then will terminate itself ''' def callback(config): rospy.loginfo("""Reconfigure Request: {Joint_1}, {Joint_2},\ {Joint_3}, {Joint_4}, {Joint_5},{Joint_6},{Activate}, {Execute}""".format(**config)) def callback2(config): rospy.loginfo(("""Reconfigure Request: {x}, {y},\ {z}, {phi}, {theta},{epsi},{Activate},{Execute}""".format(**config))) #callback function for robot pos joints_position = Floats() def pos_callback(robot_joints_pos): global joints_position joints_position = robot_joints_pos.data #print("pose : ",joints_position[0]) cartesian_pos = Floats() def cartes_callback(robot_cartesian_pos): global cartesian_pos cartesian_pos = robot_cartesian_pos.data print("cartesian :", robot_cartesian_pos)
# -*- coding: utf-8 -*- """ Created on Tue Jun 9 17:19:54 2020 @author: varun """ #!C:/Python/python.exe import rospy from rospy.numpy_msg import numpy_msg from rospy_tutorials.msg import Floats arr = Floats() def callback(data): global arr arr = data rospy.init_node("arraySub", anonymous=True) rospy.Subscriber("/vp_array", numpy_msg(Floats), callback) while True: print(arr)
if flag == 0: in_scene = False elif flag == 1: in_scene = True return in_scene #connecting to robot rob = urx.Robot("172.31.1.3") rob.set_tcp((0, 0, 0.185, 0, 0, 0)) sleep(1) #leave some time to robot to process the setup commands #callback function subscribes to robot pose cartesian_pos = Floats() def cartes_callback(robot_cartesian_pos): global cartesian_pos cartesian_pos = robot_cartesian_pos.data # print("cartesian :", robot_cartesian_pos) #callback function subscribing to ball pose ball_pose = Point() def ball_callback(position_ball): global ball_pose ball_pose = position_ball
codeDir = "C:\\Users\\varun\\Desktop\\NCSU\\eccv_indoor-master\\eccv_indoor-master\\UIUC_Varsha\\SpatialLayout\\spatiallayoutcode\\" imgDir = "C:\\Users\\varun\\Desktop\\NCSU\\eccv_indoor-master\\Cell Phone\\" workDir = "C:\\Users\\varun\\Desktop\\NCSU\\eccv_indoor-master\\vanishing_point\\" ## Start ROS node rospy.init_node('vanishingPoint', anonymous=True) pub = rospy.Publisher("/vp_array", numpy_msg(Floats),queue_size=0) r = rospy.Rate(10) # 10hz ## Adding the path into the python interpreter eng.addpath(codeDir,nargout=0) eng.addpath(codeDir+"ComputeVP\\",nargout=0) i = 1 while not rospy.is_shutdown() and i < 9343: try: imageName = "img"+str(i)+".jpg" i+=10 #_, frame = cam.read() #cv2.imwrite(imgDir+imageName, cv2.resize(frame, (640,360))) (boxlayout, surface_labels, resizefactor, vpdata) = eng.getspatiallayout(imgDir, imageName, workDir, nargout=4) arr = np.asarray(vpdata['vp']).astype('float').flatten() send = Floats() send.data = list(arr) pub.publish(send) r.sleep() except matlab.engine.MatlabExecutionError: print("Okay, I think something just went wrong there !")