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 !")