def rectify_stitch_stereo_image(image_left, image_right, info_left,
info_right):
cam_left = image_geometry.PinholeCameraModel()
cam_left.fromCameraInfo(info_left)
cam_right = image_geometry.PinholeCameraModel()
cam_right.fromCameraInfo(info_right)
if len(image_left.shape) > 2 and image_left.shape[2] > 1:
image_left = grayscale(image_left)
image_right = grayscale(image_right)
extra_space = 200
cam_left.P[0, 2] += extra_space
warped_image_size = (image_left.shape[0],
image_left.shape[1] + extra_space)
warped_left, left_mapx = rectify_image(cam_left, image_left,
warped_image_size)
warped_right, right_mapx = rectify_image(cam_right, image_right,
warped_image_size)
stitched = np.zeros(
(warped_left.shape[0], warped_left.shape[1] + extra_space))
non_overlap_pixels = extra_space + 200
blank_pixels = 200
overlap_pixels = warped_left.shape[1] - non_overlap_pixels - blank_pixels
blend_map_linear = np.concatenate(
(np.ones(non_overlap_pixels),
np.arange(1, 0,
-1.0 / (overlap_pixels + 1))[1:], np.zeros(blank_pixels)))
stitched[:, :warped_left.shape[1]] = warped_left * blend_map_linear
stitched[:, -warped_right.shape[1]:] += warped_right * np.flip(
blend_map_linear, 0)
stitched = np.asarray(
stitched, image_left.dtype) # get the same type out as we put in
fov = np.zeros((stitched.shape[1]))
fl = np.linspace(60.6 + 27, -60.6 + 27, image_left.shape[1]).reshape(1, -1)
fr = np.linspace(60.6 - 27, -60.6 - 27,
image_right.shape[1]).reshape(1, -1)
left_mapx = left_mapx.mean(axis=0)
right_mapx = right_mapx.mean(axis=0)
fov_l = cv2.remap(fl, left_mapx, np.zeros_like(left_mapx),
cv2.INTER_CUBIC).flatten()
fov_r = cv2.remap(fr, right_mapx, np.zeros_like(right_mapx),
cv2.INTER_CUBIC).flatten()
fov[:fov_l.size] = fov_l * blend_map_linear
fov[-fov_r.size:] += fov_r * np.flip(blend_map_linear, 0)
return stitched, fov
def __init__(self):
rospy.init_node('active_detection')
self.tf_listener = tf.TransformListener()
################ Init Database Services ###############################
rospy.wait_for_service('/database/requests')
self.DBQuery_proxy = rospy.ServiceProxy('/database/requests', ObjectDBQuery)
################ Init Publishers and Bridge ###########################
# Image publisher for debugging, publishes images with bounding boxes
# and labels drawn around detected objects.
# Debug image for verification
self.image_pub = rospy.Publisher(
"/cvod/debug_images", Image, queue_size=1)
'''
CVBridge allows us to convert openCV images into ROS msgs and vice versa.
It is the active link between publishers, subscribers and the operations performed between.
'''
self.bridge = CvBridge()
################ Init Subscribers #####################################
# Used to keep track of when we should update PCODAR.
self.see_frame_counter = 0
self.left_frame_counter = 0
# Image subscriber for seecam, these images will be processed by the
# network.
self.see_sub = mil_tools.Image_Subscriber(
topic="/camera/seecam/image_raw", callback=self.callback, image_callback_args="/seecam")
# Image subscriber for front left camera, these images will be
# processed by the network.
self.left_sub = mil_tools.Image_Subscriber(
topic="/camera/front/left/image_raw", callback=self.callback, image_callback_args="/front_left_cam")
################ Get Camera Info ######################################
#### Seecam ####
self.see_camera_info = self.see_sub.wait_for_camera_info()
self.see_img_geom = image_geometry.PinholeCameraModel()
self.see_img_geom.fromCameraInfo(self.see_camera_info)
#### Front Left ####
self.left_camera_info = self.left_sub.wait_for_camera_info()
self.left_img_geom = image_geometry.PinholeCameraModel()
self.left_img_geom.fromCameraInfo(self.left_camera_info)
def __init__(self):
rospy.init_node("Kid", anonymous=True)
# attributes for obstacle avoidance
self.distance = [10.0 for i in range(0,16)]
self.obstacles = []
# attributes for pose management and POI robot-centric localization
self.axisRotationMatrix = self.NO_ROTATION_MATRIX
self.startOrientation = 0.0
self.translationVector = self.NO_TRANSLATION_VECTOR
self.startPoint = Vector2D()
self.theta = 0.0
self.rotationMatrix = self.NO_ROTATION_MATRIX
self.POI = Vector2D()
# releasers
self.targetFound = False
self.avoidReleaser = False
self.canRead = True
self.POIFound = False
# temporal releasers
self.lastPOIFound = -self.MAX_TIME_ELAPSED
self.lastPoseUpdateTime = -self.POSE_UPDATE_RATE
self.lastLookUpdateTime = -self.LOOK_UPDATE_RATE
self.lastSonarReadTime = -self.SONAR_UPDATE_RATE
self.lastRandomField = -self.RANDOM_FIELD_RATE
self.msg = Twist()
self.inGame = False
self.colorToTouch = ""
self.colorTouched = False
# ROS library needed for image conversion from ROS to OpenCV
self.bridge = CvBridge()
# Needed to interpretate images geometrically with camera parameters
self.camera_model = image_geometry.PinholeCameraModel()
self.frameRGBD = None
self.poseInfo = None
# publishers and subscribers
self.velPub = rospy.Publisher("/RosAria/cmd_vel", Twist, queue_size=1)
self.sonarSub = rospy.Subscriber("/RosAria/sonar", PointCloud, self.readSonar)
self.poseSub = rospy.Subscriber("/RosAria/pose", Odometry, self.updatePose)
self.RMSpeakerPub = rospy.Publisher("node_to_rolemanager", String, queue_size=10)
self.RMListenerSub = rospy.Subscriber("rolemanager_to_node", String, self.ownRoleManagerListener)
# Subscriber for computer vision module
self.imageSub = message_filters.Subscriber("/camera/rgb/image_rect_color", Image)
self.depthSub = message_filters.Subscriber("/camera/depth_registered/image", Image)
self.cameraInfoSub = message_filters.Subscriber("/camera/depth_registered/camera_info", CameraInfo)
# Topic time synchronizer
ats = message_filters.ApproximateTimeSynchronizer([self.imageSub,self.depthSub,self.cameraInfoSub], queue_size = 10, slop = 0.1)
ats.registerCallback(self.RGBDSubscriber)
# Color target
self.colorlower = None
self.colorupper = None
def __init__(self):
# ========== TF ======== #
# TF Listner #
self.tf_buffer = tf2_ros.Buffer()
self.tf_listner = tf2_ros.TransformListener(self.tf_buffer)
# ======== Transform Info ======== #
self.trans = list()
# ======== BoundingBox Callback ======== #
self.bbox_sub = rospy.Subscriber('/clustering_result',
BoundingBoxArray,
self.bbArrayCb,
queue_size=1)
# ======= Camera Callback ======== #
self.img_sub = rospy.Subscriber('/kinect_second/hd/image_color', Image,
self.imgCb)
self.cam_sub = rospy.Subscriber('/kinect_second/hd/camera_info',
CameraInfo, self.camInfoCb)
self.bridge = CvBridge()
# ======== Camera Model ======== #
self.camera_model = image_geometry.PinholeCameraModel()
# ======== ImageArray Publisher ======== #
self.img_array_pub = rospy.Publisher('/bbox_cap_img_array',
ImageArray,
queue_size=1)
def cameraRGBInfoCallback(self, data):
if self.cameraWidth is None:
self.cameraWidth = data.width
self.cameraHeight = data.height
self.pinholeCamera = image_geometry.PinholeCameraModel()
self.pinholeCamera.fromCameraInfo(data)
self.rgbCameraFrame = data.header.frame_id
def adjust_fold(self, req):
#Go to viewing stance
StanceUtils.call_stance("open_both", 5.0)
StanceUtils.call_stance("viewing", 5.0)
last_model = pickle.load(open("/tmp/last_model.pickle"))
camera_model = image_geometry.PinholeCameraModel()
info = RosUtils.get_next_message("wide_stereo/left/camera_info",
CameraInfo)
cam_frame = info.header.frame_id
camera_model.fromCameraInfo(info)
now = rospy.Time.now()
req.start.header.stamp = now
req.end.header.stamp = now
self.listener.waitForTransform(cam_frame, req.start.header.frame_id,
now, rospy.Duration(20.0))
start_cam = self.listener.transformPoint(cam_frame, req.start)
end_cam = self.listener.transformPoint(cam_frame, req.end)
start = camera_model.project3dToPixel(
(start_cam.point.x, start_cam.point.y, start_cam.point.z))
end = camera_model.project3dToPixel(
(end_cam.point.x, end_cam.point.y, end_cam.point.z))
folded_model = Models.Point_Model_Folded(last_model, start, end)
folded_model.image = None
folded_model.initial_model.image = None
pickle.dump(folded_model, open("/tmp/last_model.pickle", 'w'))
adjust_folded_model = rospy.ServiceProxy(
"fold_finder_node/process_mono", ProcessMono)
resp = adjust_folded_model("wide_stereo/left")
new_start = resp.pts3d[0]
new_end = resp.pts3d[1]
return AdjustFoldResponse(start=new_start, end=new_end)
def __init__(self):
cv2.namedWindow('Gesture Detector', cv2.WINDOW_NORMAL)
cv2.createTrackbar("Lower Hue", 'Gesture Detector', 0, 180, update_mask)
cv2.createTrackbar("Upper Hue", 'Gesture Detector', 0, 180, update_mask)
cv2.createTrackbar("Lower Saturation", 'Gesture Detector', 0, 255, update_mask)
cv2.createTrackbar("Upper Saturation", 'Gesture Detector', 0, 255, update_mask)
cv2.createTrackbar("Lower Value", 'Gesture Detector', 0, 255, update_mask)
cv2.createTrackbar("Upper Value", 'Gesture Detector', 0, 255, update_mask)
#cv2.createTrackbar("Pump Enable", "Camera", 0, 1, nothing)
#Good default values
cv2.setTrackbarPos("Lower Hue", 'Gesture Detector', 136)
cv2.setTrackbarPos("Upper Hue", 'Gesture Detector', 180)
cv2.setTrackbarPos("Lower Saturation", 'Gesture Detector', 50)
cv2.setTrackbarPos("Upper Saturation", 'Gesture Detector', 176)
cv2.setTrackbarPos("Lower Value", 'Gesture Detector', 0)
cv2.setTrackbarPos("Upper Value", 'Gesture Detector', 135)
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.kinect_model = image_geometry.PinholeCameraModel()
self.pump_pub = rospy.Publisher('/uarm/pump', Bool, queue_size=10)
rospy.Subscriber('camera/rgb/camera_info', CameraInfo, self.get_camera_info)
rospy.Subscriber('/camera/rgb/image_color', Image, self.image_callback)
self.pump_msg = Bool()
def __init__(self):
rospy.init_node('Kinect', anonymous=True)
# Subscribers
self.imageSub = message_filters.Subscriber(
"/camera/rgb/image_rect_color", Image)
self.depthSub = message_filters.Subscriber(
"/camera/depth_registered/image", Image)
self.cameraInfoSub = message_filters.Subscriber(
"/camera/depth_registered/camera_info", CameraInfo)
# Topic time synchronizer
ats = message_filters.ApproximateTimeSynchronizer(
[self.imageSub, self.depthSub, self.cameraInfoSub],
queue_size=10,
slop=0.1)
ats.registerCallback(self.imageCapture)
# ROS library needed for image conversion from ROS to OpenCV
self.bridge = CvBridge()
# Needed to interpretate images geometrically with camera parameters
self.camera_model = image_geometry.PinholeCameraModel()
# Color init
lightblue = np.uint8([[[204, 204, 0]]])
hsv_lightblue = cv2.cvtColor(lightblue, cv2.COLOR_BGR2HSV)
self.colorlower = np.array([hsv_lightblue[0][0][0] - CONST, 50, 50])
self.colorupper = np.array([hsv_lightblue[0][0][0] + CONST, 255, 255])
def __init__(self, camera_name, rgb_topic, depth_topic, camera_info_topic):
self.camera_name = camera_name
self.rgb_topic = rgb_topic
self.depth_topic = depth_topic
self.camera_info_topic = camera_info_topic
self.pose = None
self.marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
# cv2.namedWindow("Image window", cv2.WINDOW_NORMAL)
# cv2.setMouseCallback("Image window", self.mouse_callback)
self.br = tf.TransformBroadcaster()
#Have we recieved camera_info and image yet?
self.ready_ = False
self.bridge = CvBridge()
self.camera_model = image_geometry.PinholeCameraModel()
rospy.loginfo('Camera {} initialised, {}, , {}'.format(self.camera_name, rgb_topic, depth_topic, camera_info_topic))
print('')
q=25
self.sub_rgb = message_filters.Subscriber(rgb_topic, Image, queue_size=q)
self.sub_depth = message_filters.Subscriber(depth_topic, Image, queue_size=q)
self.sub_camera_info = message_filters.Subscriber(camera_info_topic, CameraInfo, queue_size=q)
# self.tss = message_filters.ApproximateTimeSynchronizer([self.sub_rgb, self.sub_depth, self.sub_camera_info], queue_size=15, slop=0.4)
self.tss = message_filters.ApproximateTimeSynchronizer([self.sub_rgb, self.sub_depth, self.sub_camera_info], queue_size=30, slop=0.5)
#self.tss = message_filters.TimeSynchronizer([sub_rgb], 10)
self.tss.registerCallback(self.callback)
def __init__(self):
rospy.loginfo("Creating Obj3DDetector class")
# flags and vars
self.ph_model = image_geometry.PinholeCameraModel()
self.tracked_pixel = Point()
self.use_kb = False
self.ball_radius = 0
self.p_ball_to_cam = []
self.bridge = CvBridge()
self.tb_val = self.window_with_trackbars('image_red', GREEN_TB_DEFAULTS, GREEN_TB_HIGHS)
# Get and set params
self.get_and_set_params()
# kbhit instance
self.kb = kbhit.KBHit()
if self.use_kb:
self.print_help()
rospy.on_shutdown(self.kb.set_normal_term)
self.imwritecounter = 0
# subscribers
self.cam_info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.update_model_cb)
self.msg_filt_rgb_img_sub = message_filters.Subscriber("/camera/rgb/image_color", Image)
self.msg_filt_depth_img_sub = message_filters.Subscriber("/camera/depth_registered/hw_registered/image_rect", Image)
self.t_sync = message_filters.ApproximateTimeSynchronizer([self.msg_filt_rgb_img_sub, self.msg_filt_depth_img_sub], 1, slop = 0.01)
self.t_sync.registerCallback(self.time_sync_img_cb)
# publishers and timers
self.kb_timer = rospy.Timer(rospy.Duration(0.1), self.keycb)
self.start_time = 0
self.tf_br = tf.TransformBroadcaster()
def __init__(self,
topic_rect=params["Camera"]["topic_rect"],
topic_info=params["Camera"]["topic_info"]):
"""
Camera class for updating camera frames, camera information and publising the debug image.
Params:
topic_rect: the ROS topic of image_rect
topic_info: the ROS topic of camera_info
"""
rospy.init_node('camera', anonymous=True)
self.frame_rect = None
self.debugImage = None
self.debugImageOK = False
self.camera_info_msg = None
self.bridge = CvBridge()
self.camera_rect = rospy.Subscriber(topic_rect, Image,
self.callback_rect)
self.debugImagePub = rospy.Publisher("debugImage", Image, queue_size=2)
self.camera_model = image_geometry.PinholeCameraModel()
self.setAutoExposure(True)
self.setAutoFrameRate(True)
self.changeSubsampling(1)
time.sleep(0.5)
self.camera_info = rospy.Subscriber(topic_info, CameraInfo,
self.callback_info)
rospy.wait_for_message(params["Camera"]["topic_info"], CameraInfo)
rospy.wait_for_message(params["Camera"]["topic_rect"], Image)
time.sleep(0.5)
self.camera_model.fromCameraInfo(self.camera_info_msg)
def __init__(self, num_frames, hand_coords):
# self.pointcloud_sub = message_filters.Subscriber("/depth_registered/points",PointCloud2)
self.caminfo_sub = message_filters.Subscriber("/camera_info_rgb",
CameraInfo)
self.frame_number_sub = message_filters.Subscriber(
"/frame_number", PointStamped)
self.sequence_sync = message_filters.ApproximateTimeSynchronizer(
[self.caminfo_sub, self.frame_number_sub],
20,
1,
allow_headerless=False)
self.sequence_sync.registerCallback(self.callback)
self.num_frames = num_frames
self.hand2d = hand_coords
self.hand_coords = npy.zeros((self.num_frames, 4, 3))
self.pixel_width = 10
self.size_var = 2 * self.pixel_width + 1
x = npy.zeros((self.size_var, self.size_var))
x[self.pixel_width, self.pixel_width] = 1.
self.gf = gfx(x, 2)
# print(self.gf)
self.img_geo = ig.PinholeCameraModel()
def __init__(self):
rospy.loginfo('Waiting for camera info')
self.camera_info = rospy.wait_for_message('camera_info', CameraInfo)
print('camera_info:\n%s' % self.camera_info)
self.geometry_camera = image_geometry.PinholeCameraModel()
self.geometry_camera.fromCameraInfo(self.camera_info)
self.optical_frame = self.geometry_camera.tf_frame
self.tf_listener = tf.TransformListener()
self.image_pub = rospy.Publisher('image_project', Image, queue_size=1)
self.image_sub = rospy.Subscriber('image_raw',
Image,
self.on_image,
queue_size=1)
self.points3d_service = rospy.Service('project_points3d',
SetProjectPoints3D,
self.on_project_points3d)
self.points2d_service = rospy.Service('project_points2d',
SetProjectPoints2D,
self.on_project_points2d)
self.tf_service = rospy.Service('project_tf', SetProjectTFs,
self.on_project_tfs)
self.last_cv2_image = None
self.overlay_image = None
self.overlay_mask = None
self.points2d = None
self.points3d = None
self.tf = None
def __init__(self, parent):
super(KinectTracker, self).__init__(parent)
self.name = self.__class__.__name__
# Ros Stuff
self.sub_depth = None #Initialised later
self.bridge = CvBridge()
self.pub_depth = rospy.Publisher("/camera/detector/image_raw",
ImageMSG,
queue_size=1)
self.pub_camera = rospy.Publisher("/camera/detector/camera_info",
CamInfoMSG,
queue_size=1)
# Parameters
self.method = None
self.setMethod(1)
# Maximum depth we consider
self.maxDepth = None
self.setMaxDepth(m=4.5)
# How far away we need to be from the background to be considered foreground
self.bg_thresh = None
self.setForegroundDist(cm=45)
# Size of kernel for morphalogical operations
self.kernel = None
self.setKernel(2) #x*2+1
# Number of morphalogical operation iterations
self.morphIterations = None
self.setIterations(1)
# Camera matrix properties #TODO: use numpy and an intrinsic camera matrix to matrix-point multiply
# FOR NOW USING ROS CAMERA MODEL
#self.centerX = 319.5
#self.centerY = 239.5
#self.depthFocalLength = 525
# Size restrictions of detected object (metric)
self.estSize = None
self.setSize(cm=6)
self.estSizeTol = None
self.setSizeTolerance(cm=4)
self.priority = None # 0: blob size, 1:pixel coord, 2:x, 3:y, 4:z, 5: object width, 6:difference from background
self.setPriority(0)
self.depthEstMode = None
self.setDepthEstMode(0)
self.goal = [] # x,y,z in camera frame
# Background History
self.acc = None
self.counterSteps = -1
self.counter = 0
#self.resetBackground(3.5)
# Image buffers
self.depth = None # current depth image [float32]
self.show = None # image to display and draw on [rgb8]
self.method = 0 #0 = median, 1 = minimum
self.cameraModel = image_geometry.PinholeCameraModel()
self.cameraInfo = None
def get_pinhole_camera_model(bag_fname, infotopic):
""" assuming model is static """
bag = rosbag.Bag(bag_fname, 'r')
for topic, msg, bts in bag.read_messages(infotopic):
cam = image_geometry.PinholeCameraModel()
cam.fromCameraInfo(msg)
break
return cam
def convert_imgmsg(self, cammsg, caminfomsg):
cvImg = self.bridge.imgmsg_to_cv2(cammsg,
desired_encoding='passthrough')
cam_model = image_geometry.PinholeCameraModel()
cam_model.fromCameraInfo(caminfomsg)
cam_model.rectifyImage(cvImg, 0)
cvImg = cv2.remap(cvImg, cam_model.mapx, cam_model.mapy, 1)
return cvImg, cam_model
def setCameraModel(self, msg):
"""
thorvald_001/kinect2_camera/hd/camera_info topic callback Type: CameraInfo
Used to get camera characteristics for PinHoleCameraModel then unsubscribes as not needed once values
have been set.
"""
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(msg)
self.camera_info_sub.unregister()
def image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
def __init__(self):
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.tf2Buffer = tf2_ros.Buffer()
self.tf2listener = tf2_ros.TransformListener(self.tf2Buffer)
self.br = tf.TransformBroadcaster()
self.camera_info = rospy.wait_for_message(
'/thorvald_002/kinect2_camera/hd/camera_info', CameraInfo)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
def __init__(self):
self.bridge = CvBridge()
self.listener = tf.TransformListener()
self.tf2Buffer = tf2_ros.Buffer()
self.tf2listener = tf2_ros.TransformListener(self.tf2Buffer)
self.image_pub = rospy.Publisher("only_weeds_image",Image, queue_size=10)
self.pose_pub = rospy.Publisher('weed_to_kill', PoseStamped, queue_size=1)
self.br = tf.TransformBroadcaster()
self.camera_info = rospy.wait_for_message('/thorvald_001/kinect2_camera/hd/camera_info', CameraInfo)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
def __init__(self):
grid_spacing = rospy.get_param('~grid_spacing')
self.bridge = CvBridge()
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('projector/get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'projector/get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('projector/set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('projector/set_projection',
projector.srv.SetProjection)
projector_info = projector_info_service().projector_info
projector_model = image_geometry.PinholeCameraModel()
projector_model.fromCameraInfo(projector_info)
# Generate grid projections
self.original_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
for row in range(0, projector_info.height, grid_spacing):
cv.Line(self.original_projection, (0, row),
(projector_info.width - 1, row), 255)
for col in range(0, projector_info.width, grid_spacing):
cv.Line(self.original_projection, (col, 0),
(col, projector_info.height - 1), 255)
predistortmap_x = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
InitPredistortMap(projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs(), predistortmap_x,
predistortmap_y)
self.predistorted_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
cv.Remap(self.original_projection,
self.predistorted_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=(0, 0, 0, 0))
self.off_projection = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_8UC1)
cv.SetZero(self.off_projection)
def __init__(self):
self.base_link = 'base_link'
self.ee_link = 'ee_link'
flag, self.tree = kdl_parser.treeFromParam('/robot_description')
self.chain = self.tree.getChain(self.base_link, self.ee_link)
self.num_joints = self.tree.getNrOfJoints()
self.vel_ik_solver = kdl.ChainIkSolverVel_pinv(self.chain)
self.pos_fk_solver = kdl.ChainFkSolverPos_recursive(self.chain)
self.pos_ik_solver = kdl.ChainIkSolverPos_LMA(self.chain)
self.cam_model = image_geometry.PinholeCameraModel()
self.joint_state = kdl.JntArrayVel(self.num_joints)
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
rospy.init_node('ur_eef_vel_controller')
rospy.Subscriber('/joint_states', JointState, self.arm_joint_state_cb)
rospy.Subscriber('/rdda_interface/joint_states', JointState,
self.finger_joint_state_cb)
self.joint_vel_cmd_pub = rospy.Publisher(
'/joint_group_vel_controller/command',
Float64MultiArray,
queue_size=10)
self.joint_pos_cmd_pub = rospy.Publisher(
'/scaled_pos_traj_controller/command',
JointTrajectory,
queue_size=10)
self.speed_scaling_pub = rospy.Publisher('/speed_scaling_factor',
Float64,
queue_size=10)
self.switch_controller_cli = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
self.joint_pos_cli = actionlib.SimpleActionClient(
'/scaled_pos_joint_traj_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
cam_info = rospy.wait_for_message('/camera/depth/camera_info',
CameraInfo,
timeout=10)
self.cam_model.fromCameraInfo(cam_info)
self.bridge = CvBridge() # allows conversion from depth image to array
self.tf_listener = tf.TransformListener()
self.image_offset = 100
self.pos_ctrler_running = False
rospy.sleep(0.5)
def info2cvmat(info):
'''
converts a ros CameraInfo msg to cv mats
return: (cameraMatrix, distCoeffs, projectionMatrix, rotationMatrix)
'''
phc = image_geometry.PinholeCameraModel();
phc.fromCameraInfo(info);
cameraMatrix = phc.intrinsicMatrix();
distCoeffs = phc.distortionCoeffs();
projectionMatrix = phc.projectionMatrix();
rotationMatrix = phc.rotationMatrix();
return (cameraMatrix, distCoeffs, projectionMatrix, rotationMatrix);
def image_cb(self, image):
'''Hang on to last image'''
self.last_image = image
self.last_image_time = self.image_sub.last_image_time
if self.camera_model is None:
if self.image_sub.camera_info is None:
return
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.image_sub.camera_info)
self.ppf = ProjectionParticleFilter(self.camera_model,
max_Z=15,
salting=0.05,
jitter_prob=0)
def __init__(self):
self.printed_chessboard_corners_x = rospy.get_param(
'~printed_chessboard_corners_x')
self.printed_chessboard_corners_y = rospy.get_param(
'~printed_chessboard_corners_y')
self.printed_chessboard_spacing = rospy.get_param(
'~printed_chessboard_spacing')
self.bridge = CvBridge()
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info',
sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection',
projector.srv.SetProjection)
rospy.loginfo("Waiting for projector info setting service...")
rospy.wait_for_service('set_projector_info')
rospy.loginfo("Projection setting service found.")
self.set_projector_info = rospy.ServiceProxy(
'set_projector_info', sensor_msgs.srv.SetCameraInfo)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.projector_info = projector_info_service().projector_info
self.blank_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.SetZero(self.blank_projection)
self.number_of_scenes = 0
self.object_points = []
self.image_points = []
def find_object(r,g,b):
imgdata=rospy.wait_for_message('camera/image_raw',Image)
if(imgdata is None):
print "error reciving image from camera"
return
bridge = CvBridge()
image = bridge.imgmsg_to_cv2(imgdata,"bgr8")
camInfodata=rospy.wait_for_message('camera/camera_info',CameraInfo)
if(camInfodata is None):
print "error reciving caminfo"
return
camInfo=camInfodata
scannerdata=rospy.wait_for_message('scan',LaserScan)
if(scannerdata is None):
print "error getting scanner data"
return
distances=scannerdata
if(camInfo!=0 and distances!=0 and len(image)!=0):
camera = image_geometry.PinholeCameraModel()
camera.fromCameraInfo(camInfo)
Radius_center=(0,0)
Radius_center = findCenter(image,r,g,b)
if(Radius_center==None):
print "OBJECT NOT FOUND!"
msg = Float32()
#print "hereee"
msg.data =-1.0
#pub.publish(msg)
return None
else:
ray = camera.projectPixelTo3dRay(camera.rectifyPoint(Radius_center))
#print("x = "+str(ray[0])+" y= "+str(ray[1])+" z= "+str(ray[2])+")")
alpha = np.dot(ray[0],ray[2])
#print("alpha:"+str(math.degrees(alpha)))
if(alpha < 0):
alpha = -alpha
else:
alpha = math.floor(math.pi * 2 - alpha)
#print "min"+str(distances.angle_min)
#print "increment"+str(distances.angle_increment)
distance_index = int((alpha - distances.angle_min) / distances.angle_increment)
#print "index"+str(distance_index)
actual_distance = distances.ranges[distance_index]
#print actual_distance
print "the distance to the object is "+str(actual_distance)
return actual_distance
def __init__(self, filename=None):
if filename is None:
return
self.filename = filename
with open(self.filename, 'rb') as f:
support_surface = pickle.load(f)
assert(isinstance(support_surface, support_surface_object.SupportSurface))
self.image = support_surface.image
self.depth = support_surface.depth
self.camera = image_geometry.PinholeCameraModel()
self.camera.fromCameraInfo(support_surface.camera_info)
self.surface_normal = np.array(support_surface.surface_normal)
def __init__(self):
# A generalised Pinhole camera model provided by the image_geometry module in ROS. Used as an approximate model
# of the drone camera. Should be modified with correct camera parameters before usage.
self.drone_camera = ig.PinholeCameraModel()
# A buffer which is then used for the TransformListener provided by the ROS tf2 library. Used to queue up
# static 3-D coordinates of objects and dynamic 3-D coordinates of the drone. Can be used to transform object
# coordinates to the drone's frame of reference
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
self.transformer = tf.TransformerROS()
# A bridging library used to transform ROS images to OpenCV images
self.bridge = CvBridge()
def find_object(r, g, b):
print "find object"
imgdata = rospy.wait_for_message('camera/image_raw', Image)
if (imgdata is None):
print "error reciving image from camera"
return None, None
bridge = CvBridge()
image = bridge.imgmsg_to_cv2(imgdata, "bgr8")
print "we have image :)"
camInfodata = rospy.wait_for_message('camera/camera_info', CameraInfo)
if (camInfodata is None):
print "error reciving caminfo"
return None, None
camInfo = camInfodata
print "camInfo:" + str(camInfo.K[0]) + str(camInfo.K[1]) + str(
camInfo.K[2])
scannerdata = rospy.wait_for_message('scan', LaserScan)
if (scannerdata is None):
print "error getting scanner data"
return None, None
distances = scannerdata
if (camInfo != 0 and distances != 0 and len(image) != 0):
camera = image_geometry.PinholeCameraModel()
camera.fromCameraInfo(camInfo)
Radius_center = (0, 0)
Radius_center = findCenter(image, r, g, b)
if (Radius_center == None):
print "OBJECT NOT FOUND!"
msg = Float32()
msg.data = -1.0
return None, None
else:
ray = camera.projectPixelTo3dRay(
camera.rectifyPoint(Radius_center))
alpha = np.dot(ray[0], ray[2])
if (alpha < 0):
alpha = -alpha
else:
alpha = math.floor(math.pi * 2 - alpha)
distance_index = int(
(alpha - distances.angle_min) / distances.angle_increment)
actual_distance = distances.ranges[distance_index]
print "the distance to the object is " + str(actual_distance)
return actual_distance, alpha
def __init__(self,num_frames, hand_coords, FILE_DIR):
self.FILE_DIR = FILE_DIR
self.caminfo_sub = message_filters.Subscriber("/camera_info_rgb",CameraInfo)
self.frame_number_sub = message_filters.Subscriber("/frame_number",PointStamped)
self.depth_image_sub = message_filters.Subscriber("/depth_image",Image)
self.sequence_sync = message_filters.ApproximateTimeSynchronizer([self.caminfo_sub,self.frame_number_sub,self.depth_image_sub],20,1,allow_headerless=False)
self.sequence_sync.registerCallback(self.callback)
self.num_frames = num_frames
self.hand2d = hand_coords
self.hand_coords = npy.zeros((self.num_frames,4,3))
self.pixel_width = 0
self.size_var = 2*self.pixel_width+1
self.img_geo = ig.PinholeCameraModel()
self.bridge = CvBridge()
