def main():
args = parse_args()
left_cm = CameraInfoManager(namespace='left')
left_cm.setURL('file://{}'.format(os.path.abspath(args.l)))
left_cm.loadCameraInfo()
left_msg = left_cm.getCameraInfo()
right_cm = CameraInfoManager(namespace='right')
right_cm.setURL('file://{}'.format(os.path.abspath(args.r)))
right_cm.loadCameraInfo()
right_msg = right_cm.getCameraInfo()
in_bag = rosbag.Bag(args.bagfile)
in_basename = os.path.splitext(
os.path.basename(os.path.abspath(args.bagfile)))[0]
out_filepath = os.path.join(os.path.dirname(os.path.abspath(args.bagfile)),
in_basename + '_wcaminfo.bag')
print 'Writing to', out_filepath
with rosbag.Bag(out_filepath, 'w') as out_bag:
for topic, msg, t in in_bag.read_messages():
if topic == '/cam0/image_raw':
left_msg.header = msg.header
out_bag.write('/cam/left/camera_info', left_msg, t)
out_bag.write('/cam/left/image_raw', msg, t)
elif topic == '/cam1/image_raw':
right_msg.header = msg.header
out_bag.write('/cam/right/camera_info', right_msg, t)
out_bag.write('/cam/right/image_raw', msg, t)
else:
out_bag.write(topic, msg, t)
def __init__(self):
rospy.init_node('{}_node'.format(CAMERA_NAME), argv=sys.argv)
self.device = rospy.get_param('~device', '/dev/ttyACM0')
self.image_topic = rospy.get_param('~image', DEFAULT_IMAGE_TOPIC)
self.camera_topic = rospy.get_param('~camera', DEFAULT_CAMERA_TOPIC)
self.calibration = rospy.get_param('~calibration', '')
self.manager = CameraInfoManager(cname=CAMERA_NAME,
url='file://' + self.calibration,
namespace=CAMERA_NAME)
self.manager.loadCameraInfo() # Needs to be called before getter!
self.camera_info = self.manager.getCameraInfo()
self.openmv_cam = OpenMVCam(self.device)
self.bridge = CvBridge()
self.image_publisher = rospy.Publisher(self.image_topic,
Image,
queue_size=1)
self.camera_publisher = rospy.Publisher(self.camera_topic,
CameraInfo,
queue_size=1)
self.seq = 0
def main():
"""Main routine to run DOPE"""
# Initialize ROS node
# rospy.init_node('dope')
dopenode = DopeNode()
image_path = \
"/media/aditya/A69AFABA9AFA85D9/Cruzr/code/Dataset_Synthesizer/Test/Zed/NewMap1_turbosquid_can_only/000000.left.png"
# image_path = \
# "/media/aditya/A69AFABA9AFA85D9/Cruzr/code/Dataset_Synthesizer/Test/Zed/NewMap1_dope/000001.left.png"
camera_ns = rospy.get_param('camera', 'dope/webcam')
info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(0),
namespace=camera_ns)
try:
camera_info_url = rospy.get_param('~camera_info_url')
if not info_manager.setURL(camera_info_url):
rospy.logwarn('Camera info URL invalid: %s', camera_info_url)
except KeyError:
# we don't have a camera_info_url, so we'll keep the
# default ('file://${ROS_HOME}/camera_info/${NAME}.yaml')
pass
info_manager.loadCameraInfo()
if not info_manager.isCalibrated():
rospy.logwarn('Camera is not calibrated, please supply a valid camera_info_url parameter!')
camera_info = info_manager.getCameraInfo()
dopenode.run_on_image(image_path, camera_info)
def __init__(self, args):
super().__init__('raspicam_ros2')
# vars
self._topic_name = 'raspicam'
if (len(args) > 1):
self._topic_name = args[1]
self._camera_info_manager = CameraInfoManager(self,
'raspicam',
namespace='/' +
self._topic_name)
camera_info_url = 'file://' + os.path.dirname(
os.path.abspath(__file__)) + '/config/raspicam_416x320.yaml'
# pubs
self._img_pub = self.create_publisher(
Image,
'/' + self._topic_name + '/image',
qos_profile=rclpy.qos.qos_profile_sensor_data)
self._camera_info_pub = self.create_publisher(
CameraInfo, '/' + self._topic_name + '/camera_info')
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
def d5100_image_capture():
global image_raw_publisher
global camera_info_publisher
global camera_info
rospy.init_node(name())
# Tell camera to write images to RAM instead of media card
try:
subprocess.check_call(
["gphoto2", "--set-config", "/main/settings/capturetarget=0"])
except subprocess.CalledProcessError as e:
rospy.logfatal("Could not configure camera: %s", e)
return
image_raw_publisher = rospy.Publisher("image_raw", Image, queue_size=10)
camera_info_publisher = rospy.Publisher("camera_info",
CameraInfo,
queue_size=5)
rospy.Service('request_image', GetPolledImage, capture_image)
# set_camera_info service
camera_info_url = 'package://camera_driver/calibrations/%s.yaml' % name()
camera_info_manager = CameraInfoManager(name(), camera_info_url, name())
camera_info_manager.loadCameraInfo()
camera_info = camera_info_manager.getCameraInfo()
rospy.loginfo("Ready")
rospy.spin()
def image_capture():
global image_publisher
global camera_info_publisher
global camera_info
rospy.loginfo("Initializing GoPro Hero4 stream...")
rospy.init_node(name())
image_publisher = rospy.Publisher("image_raw", Image, queue_size=2)
camera_info_publisher = rospy.Publisher("camera_info",
CameraInfo,
queue_size=2)
init_stream()
rospy.Service('request_image', GetPolledImage, capture_image)
# set_camera_info service
camera_info_url = 'package://camera_driver/calibrations/gopro4.yaml'
camera_info_manager = CameraInfoManager(name(), camera_info_url)
camera_info_manager.loadCameraInfo()
camera_info = camera_info_manager.getCameraInfo()
rospy.loginfo("Ready")
rate = rospy.Rate(KEEP_ALIVE_RATE)
while True:
rospy.logdebug('Sending stream keep-alive message to gopro')
send_keep_alive()
rate.sleep()
def get_camera(self):
with Vimba.get_instance() as vimba: # noqa
cameras = vimba.get_all_cameras()
if not cameras:
rospy.logerr("Cameras were not found.")
return False
for cam in cameras:
rospy.loginfo("Camera found: " + cam.get_id())
if self._camera_id is None:
self._camera_id = cameras[0].get_id()
self._info_manager = CameraInfoManager(
cname=self._camera_id,
namespace=self._namespace,
url=
f"package://avt_camera/calibrations/{self._camera_id}.yaml"
)
elif self._camera_id not in (cam.get_id() for cam in cameras):
rospy.logerr(
f"Requested camera ID {self._camera_id} not found.")
return False
self._c0 = vimba.get_camera_by_id(self._camera_id)
rospy.loginfo(bcolors.OKGREEN +
f"Connected camera {self._camera_id}." +
bcolors.RESET)
return True
def image_capture():
global image_publisher
global camera_info_publisher
global camera_info
global camera_stream
rospy.init_node(name())
image_publisher = rospy.Publisher("image_raw", Image, queue_size=5)
camera_info_publisher = rospy.Publisher("camera_info",
CameraInfo,
queue_size=5)
camera_stream = cv2.VideoCapture('/home/marty/Downloads/GOPR0075.MP4')
rospy.Service('request_image', GetPolledImage, capture_image)
# set_camera_info service
webcam_model = rospy.get_param('~webcam_model', name())
camera_info_url = 'package://camera_driver/calibrations/%s.yaml' % webcam_model
camera_info_manager = CameraInfoManager(webcam_model, camera_info_url)
camera_info_manager.loadCameraInfo()
camera_info = camera_info_manager.getCameraInfo()
rospy.loginfo("Ready")
rospy.spin()
def __init__(self, img_pub, info_pub, namespace, camera_id):
self._img_pub = img_pub
self._info_pub = info_pub
self._bridge = CvBridge()
self._camera_id = camera_id
self._namespace = namespace
if camera_id is not None:
self._info_manager = CameraInfoManager(
cname=self._camera_id,
namespace=self._namespace,
url=f"package://avt_camera/calibrations/{self._camera_id}.yaml"
)
self._c0 = None
def __init__(self):
self.image_raw_publisher = rospy.Publisher("image_raw",
Image,
queue_size=10)
self.camera_info_publisher = rospy.Publisher("camera_info",
CameraInfo,
queue_size=5)
name = 'gopro'
if ENV_VAR_CAMERA_NAME in os.environ:
name = os.environ[ENV_VAR_CAMERA_NAME]
camera_info_url = 'package://camera_driver/calibrations/%s.yaml' % name
rospy.loginfo("Camera calibration: %s" % camera_info_url)
self.camera_info_manager = CameraInfoManager(name, camera_info_url)
self.camera_info_manager.loadCameraInfo()
self.camera_info = self.camera_info_manager.getCameraInfo()
self.sololink_config = rospy.myargv(argv=sys.argv)[1]
rospy.loginfo("Solo link config: %s" % self.sololink_config)
def unity_camera(args):
image_pub = rospy.Publisher("/ip_raw_cam/image_raw", Image, queue_size=10)
rate = rospy.Rate(40)
cinfo = CameraInfoManager(cname="camera1", namespace="/ip_raw_cam")
cinfo.loadCameraInfo()
cinfo_pub = rospy.Publisher("/ip_raw_cam/camera_info", CameraInfo, queue_size=1)
bridge = CvBridge()
data_reader = DataReader(args['hostname'], args['port'])
while not rospy.is_shutdown():
frame = data_reader.read_frame()
image_pub.publish(bridge.cv2_to_imgmsg(frame, "bgr8"))
cinfo_pub.publish(cinfo.getCameraInfo())
rate.sleep()
def find_camera(self, vimba):
system = vimba.getSystem()
system.runFeatureCommand("GeVDiscoveryAllOnce")
rospy.sleep(0.1)
camera_ids = vimba.getCameraIds()
if not camera_ids:
rospy.logerr("Cameras were not found.")
sys.exit(1)
for cam_id in camera_ids:
rospy.loginfo("Camera found: " + cam_id)
if self._camera_id is None:
self._camera_id = camera_ids[0]
elif self._camera_id not in camera_ids:
rospy.logerr("Requested camera ID {} not found".format(self._camera_id))
sys.exit(1)
self._info_manager = CameraInfoManager(cname=self._camera_id, namespace=self._namespace,
url="package://avt_camera/calibrations/${NAME}.yaml")
self._info_manager.loadCameraInfo()
def image_capture():
global image_publisher
global camera_info_publisher
global camera_info
rospy.init_node(name())
image_publisher = rospy.Publisher("image_raw", Image, queue_size=2)
camera_info_publisher = rospy.Publisher("camera_info",
CameraInfo,
queue_size=2)
rospy.Service('request_image', GetPolledImage, capture_image)
# set_camera_info service
camera_info_url = 'package://camera_driver/calibrations/%s.yaml' % name()
camera_info_manager = CameraInfoManager(name(), camera_info_url)
camera_info_manager.loadCameraInfo()
camera_info = camera_info_manager.getCameraInfo()
rospy.loginfo("Ready")
rospy.spin()
#!/usr/bin/env python
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, CameraInfo
import rospy
from camera_info_manager import CameraInfoManager
cap = cv2.VideoCapture(0)
bridge = CvBridge()
cinfo = CameraInfoManager(
cname='camera',
url='package://robot_april_detection/config/camera_info.yaml',
namespace='camera')
cinfo.loadCameraInfo()
rospy.init_node('android_camera', anonymous=True)
image_pub = rospy.Publisher("/camera/image_raw", Image, queue_size=10)
camera_info_pub = rospy.Publisher("/camera/camera_info",
CameraInfo,
queue_size=10)
while not rospy.is_shutdown():
ret, frame_rgb = cap.read()
if not ret:
break
image_pub.publish(bridge.cv2_to_imgmsg(frame_rgb, "bgr8"))
camera_info_pub.publish(cinfo.getCameraInfo())
# cv2.imshow('frame_RGB', frame_rgb)
# k = cv2.waitKey(5) & 0xFF
def __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame',
'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery',
BatteryState,
queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image',
Image,
queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
CameraInfo,
queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber('backpack_led',
ColorRGBA,
self._set_backpack_led,
queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._say_sub = rospy.Subscriber('say',
String,
self._say_callback,
queue_size=1)
self._head_sub = rospy.Subscriber('head_angle',
Float64,
self._move_head,
queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height',
Float64,
self._move_lift,
queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
#!/usr/bin/env python
from sensor_msgs.msg import CameraInfo
from camera_info_manager import CameraInfoManager
import rospy
import rospkg
import os
if __name__ == "__main__":
rospy.init_node('camera_calibration_node')
#get the parameters for the node
camera_name = rospy.get_param('camera_name')
rospack = rospkg.RosPack()
camera_info_path = rospy.get_param('info_path',
os.path.join(rospack.get_path("success_baxter_camera_calibration"),"calibration_files","{}.yaml".format(camera_name)))
#make sure the directory exist
came_info_dir = os.path.dirname(camera_info_path)
if not os.path.exists(came_info_dir):
os.makedirs(came_info_dir)
#covert the path to URL
camera_info_path = "file://" + camera_info_path
#start the camera calibration
CameraInfoManager(cname=camera_name, url=camera_info_path)
rospy.loginfo("starting fake camera calibration node")
rospy.spin()
def __init__(self, coz):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
"""
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster()
# params
self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._footprint_frame = rospy.get_param('~footprint_frame',
'base_footprint')
self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_optical_frame = rospy.get_param('~camera_optical_frame',
'cozmo_camera')
camera_info_url = rospy.get_param('~camera_info_url', '')
# pubs
self._joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=1)
self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._battery_pub = rospy.Publisher('battery',
BatteryState,
queue_size=1)
# Note: camera is published under global topic (preceding "/")
self._image_pub = rospy.Publisher('/cozmo_camera/image',
Image,
queue_size=10)
self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info',
CameraInfo,
queue_size=10)
# subs
self._backpack_led_sub = rospy.Subscriber('backpack_led',
ColorRGBA,
self._set_backpack_led,
queue_size=1)
self._twist_sub = rospy.Subscriber('cmd_vel',
Twist,
self._twist_callback,
queue_size=1)
self._say_sub = rospy.Subscriber('say',
String,
self._say_callback,
queue_size=1)
self._head_sub = rospy.Subscriber('head_angle',
Float64,
self._move_head,
queue_size=1)
self._lift_sub = rospy.Subscriber('lift_height',
Float64,
self._move_lift,
queue_size=1)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
self._diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=1)
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
# Raise the head
self.move_head(10) # level the head
self.move_lift(0) # raise the lift
# Start the tasks
self.task = 0
self.goal_pose = self._cozmo.pose
self.action = None
self._cozmo.add_event_handler(cozmo.objects.EvtObjectAppeared,
self.handle_object_appeared)
self._cozmo.add_event_handler(cozmo.objects.EvtObjectDisappeared,
self.handle_object_disappeared)
self.front_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons2, 300,
44, 63, 63, True)
self.ramp_bottom = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles5,
100, 100, 63, 63, True)
self.ramp_top = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType02, CustomObjectMarkers.Diamonds2, 5,
5, 44, 44, True)
self.drop_spot = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles4, 70,
70, 63, 63, True)
self.back_wall = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 300,
50, 63, 63, True)
self.drop_target = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons5, 5,
5, 32, 32, True)
self.drop_clue = self._cozmo.world.define_custom_wall(
CustomObjectTypes.CustomType06, CustomObjectMarkers.Triangles3, 5,
5, 32, 32, True)
self.front_wall_pose = None
self.ramp_bottom_pose = None
self.drop_spot_pose = None
self.back_wall_pose = None
self.drop_target_pose = None
self.drop_clue_pose = None
self.cube_found = False
self.target_cube = None
self.is_up_top = False # Is Cozmo on the platform
def __init__(self):
self._cv_bridge = CvBridge()
self._laser_projector = LaserProjection()
# # Camera rectification?? To be improved: read from .yaml file
# Put here the calibration of the camera
# self.DIM = (1920, 1208)
# self.K = np.array([[693.506921, 0.000000, 955.729444], [0.000000, 694.129349, 641.732500], [0.0, 0.0, 1.0]])
# self.D = np.array([[-0.022636], [ -0.033855], [0.013493], [-0.001831]])
# self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(self.K, self.D, np.eye(3), self.K, self.DIM, cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# OR load it from a yaml file
self.cameraModel = PinholeCameraModel()
# See https://github.com/ros-perception/camera_info_manager_py/tree/master/tests
camera_infomanager = CameraInfoManager(
cname='truefisheye',
url='package://ros_camera_lidar_calib/cfg/truefisheye800x503.yaml'
) # Select the calibration file
camera_infomanager.loadCameraInfo()
self.cameraInfo = camera_infomanager.getCameraInfo()
# Crete a camera from camera info
self.cameraModel.fromCameraInfo(self.cameraInfo) # Create camera model
self.DIM = (self.cameraInfo.width, self.cameraInfo.height)
# Get rectification maps
self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(
self.cameraModel.intrinsicMatrix(),
self.cameraModel.distortionCoeffs(), np.eye(3),
self.cameraModel.intrinsicMatrix(),
(self.cameraInfo.width, self.cameraInfo.height),
cv2.CV_16SC2) # np.eye(3) here is actually the rotation matrix
# # Declare subscribers to get the latest data
cam0_subs_topic = '/gmsl_camera/port_0/cam_0/image_raw'
cam1_subs_topic = '/gmsl_camera/port_0/cam_1/image_raw'
cam2_subs_topic = '/gmsl_camera/port_0/cam_2/image_raw'
#cam3_subs_topic = '/gmsl_camera/port_0/cam_3/image_raw/compressed'
lidar_subs_topic = '/Sensor/points'
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , Image, self.cam0_img_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , Image, self.cam1_img_callback, queue_size=1)
self.cam2_img_sub = rospy.Subscriber(cam2_subs_topic,
Image,
self.cam2_img_callback,
queue_size=1)
#self.cam0_img_sub = rospy.Subscriber( cam0_subs_topic , CompressedImage, self.cam0_img_compre_callback, queue_size=1)
#self.cam1_img_sub = rospy.Subscriber( cam1_subs_topic , CompressedImage, self.cam1_img_compre_callback, queue_size=1)
#self.cam2_img_sub = rospy.Subscriber( cam2_subs_topic , CompressedImage, self.cam2_img_compre_callback, queue_size=1)
#self.cam3_img_sub = rospy.Subscriber( cam3_subs_topic , CompressedImage, self.cam3_img_compre_callback, queue_size=1)
self.lidar_sub = rospy.Subscriber(lidar_subs_topic,
PointCloud2,
self.lidar_callback,
queue_size=1)
# Get the tfs
self.tf_listener = tf.TransformListener()
#self.lidar_time = rospy.Subscriber(lidar_subs_topic , PointCloud2, self.readtimestamp)
#self.img0_time = rospy.Subscriber(cam0_subs_topic , CompressedImage, self.readtimestamp)
# # Declare the global variables we will use to get the latest info
self.cam0_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam0_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam1_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam2_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_image_np = np.empty((self.DIM[1], self.DIM[0], 3))
self.cam3_undistorted_img = np.empty((self.DIM[1], self.DIM[0], 3))
self.pcl_cloud = np.empty(
(500, 4)
) # Do not know the width of a normal scan. might be variable too......
self.now = rospy.Time.now()
# # Main loop: Data projections and alignment on real time
self.lidar_angle_range_interest = [
0, 180
] # From -180 to 180. Front is 0deg. Put the range of angles we may want to get points from. Depending of camera etc
thread.start_new_thread(self.projection_calibrate())
def __init__(self,
fixed_transforms_dict=None):
self.pubs = {}
self.models = {}
self.pnp_solvers = {}
self.pub_dimension = {}
self.draw_colors = {}
self.dimensions = {}
self.class_ids = {}
self.model_transforms = {}
self.meshes = {}
self.mesh_scales = {}
self.cv_bridge = CvBridge()
self.mesh_clouds = {}
self.input_is_rectified = rospy.get_param('input_is_rectified', True)
self.downscale_height = rospy.get_param('downscale_height', 500)
self.config_detect = lambda: None
self.config_detect.mask_edges = 1
self.config_detect.mask_faces = 1
self.config_detect.vertex = 1
self.config_detect.threshold = 0.5
self.config_detect.softmax = 1000
self.config_detect.thresh_angle = rospy.get_param('thresh_angle', 0.5)
self.config_detect.thresh_map = rospy.get_param('thresh_map', 0.01)
self.config_detect.sigma = rospy.get_param('sigma', 3)
self.config_detect.thresh_points = rospy.get_param("thresh_points", 0.1)
self.downsampling_leaf_size = rospy.get_param('~downsampling_leaf_size', 0.02)
self.fixed_transforms_dict = fixed_transforms_dict
# For each object to detect, load network model, create PNP solver, and start ROS publishers
for model, weights_url in rospy.get_param('weights').items():
self.models[model] = \
ModelData(
model,
resource_retriever.get_filename(weights_url, use_protocol=False)
)
self.models[model].load_net_model()
try:
M = np.array(rospy.get_param('model_transforms')[model], dtype='float64')
self.model_transforms[model] = tf.transformations.quaternion_from_matrix(M)
except KeyError:
self.model_transforms[model] = np.array([0.0, 0.0, 0.0, 1.0], dtype='float64')
try:
self.meshes[model] = rospy.get_param('meshes')[model]
except KeyError:
pass
try:
self.mesh_scales[model] = rospy.get_param('mesh_scales')[model]
except KeyError:
self.mesh_scales[model] = 1.0
try:
cloud = PlyData.read(rospy.get_param('meshes_ply')[model]).elements[0].data
cloud = np.transpose(np.vstack((cloud['x'], cloud['y'], cloud['z'])))
if self.fixed_transforms_dict is None:
fixed_transform = np.eye(4)
else:
fixed_transform = np.transpose(np.array(self.fixed_transforms_dict[model]))
fixed_transform[:3,3] = [i/100 for i in fixed_transform[:3,3]]
# fixed_transform = np.linalg.inv(fixed_transform)
if model == "coke_bottle" or model == "sprite_bottle":
fixed_transform[1,3] = 0.172
print("Fixed transform : {}".format(fixed_transform))
cloud_pose = pcl.PointCloud()
cloud_pose.from_array(cloud)
sor = cloud_pose.make_voxel_grid_filter()
sor.set_leaf_size(self.downsampling_leaf_size, self.downsampling_leaf_size, self.downsampling_leaf_size)
cloud_pose = sor.filter()
self.mesh_clouds[model] = self.transform_cloud(np.asarray(cloud_pose), mat=fixed_transform)
# self.mesh_clouds[model] = np.asarray(cloud_pose)
# Points x 3 for dim of below
rospy.logwarn("Loaded mesh cloud for : {} with size : {}, scaling : {}".format(model, cloud.shape[0], self.mesh_scales[model]))
# scale_transform = tf.transformations.scale_matrix(self.mesh_scales[model])
# cloud = np.hstack((cloud, np.ones((cloud.shape[0], 1))))
# cloud = np.matmul(scale_transform, np.transpose(cloud))
# self.mesh_clouds[model] = np.transpose(cloud)[:, :3]
except KeyError:
rospy.logwarn("Couldn't load mesh ply")
pass
self.draw_colors[model] = tuple(rospy.get_param("draw_colors")[model])
self.dimensions[model] = tuple(rospy.get_param("dimensions")[model])
self.class_ids[model] = rospy.get_param("class_ids")[model]
self.pnp_solvers[model] = \
CuboidPNPSolver(
model,
cuboid3d=Cuboid3d(rospy.get_param('dimensions')[model])
)
self.pubs[model] = \
rospy.Publisher(
'{}/pose_{}'.format(rospy.get_param('topic_publishing'), model),
PoseStamped,
queue_size=10
)
self.pub_dimension[model] = \
rospy.Publisher(
'{}/dimension_{}'.format(rospy.get_param('topic_publishing'), model),
String,
queue_size=10
)
# Start ROS publishers
self.pub_rgb_dope_points = \
rospy.Publisher(
rospy.get_param('topic_publishing') + "/rgb_points",
ImageSensor_msg,
queue_size=10
)
self.pub_detections = \
rospy.Publisher(
'detected_objects',
Detection3DArray,
queue_size=10
)
self.pub_markers = \
rospy.Publisher(
'markers',
MarkerArray,
queue_size=10
)
self.pub_pose_cloud = \
rospy.Publisher(
rospy.get_param('topic_publishing') + "/pose_cloud",
PointCloud2,
queue_size=10
)
camera_ns = rospy.get_param('camera', 'dope/webcam')
info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(0),
namespace=camera_ns)
try:
camera_info_url = rospy.get_param('camera_info_url')
if not info_manager.setURL(camera_info_url):
rospy.logwarn('Camera info URL invalid: %s', camera_info_url)
except KeyError:
# we don't have a camera_info_url, so we'll keep the
# default ('file://${ROS_HOME}/camera_info/${NAME}.yaml')
pass
info_manager.loadCameraInfo()
self.info_manager = info_manager
self.camera_info = info_manager.getCameraInfo()
# Start ROS subscriber
# image_sub = message_filters.Subscriber(
# rospy.get_param('~topic_camera'),
# ImageSensor_msg
# )
# info_sub = message_filters.Subscriber(
# rospy.get_param('~topic_camera_info'),
# CameraInfo
# )
# ts = message_filters.TimeSynchronizer([image_sub, info_sub], 1)
# ts.registerCallback(self.image_callback)
print("Running DOPE...")
def __init__(self):
rospy.init_node('{}_node'.format(DEFAULT_CAMERA_NAME), argv=sys.argv)
self.image_topic = rospy.get_param('~image', DEFAULT_IMAGE_TOPIC)
self.camera_topic = rospy.get_param('~camera', DEFAULT_CAMERA_TOPIC)
self.calibration = rospy.get_param('~calibration', '')
self.encoding = rospy.get_param('~encoding', 'mono8')
self.width = rospy.get_param('~width', DEFAULT_WIDTH)
self.height = rospy.get_param('~height', DEFAULT_HEIGHT)
self.fps = rospy.get_param('~fps', DEFAULT_FPS)
self.cam_prod_id = rospy.get_param('~cam_prod_id', 9760)
self.contrast = rospy.get_param('~contrast', 0)
self.sharpness = rospy.get_param('~sharpness', 1)
self.gamma = rospy.get_param('~gamma', 80)
self.exposure_abs = rospy.get_param('~exposure', 50) #50 -
self.brightness = rospy.get_param('~brightness', 1)
self.hue = rospy.get_param('~hue', 0)
self.saturation = rospy.get_param('~saturation', 0)
self.pwr_line_freq = rospy.get_param('~pwr_line_freq', 1)
self.backlight_comp = rospy.get_param('~backlight_comp', 0)
self.auto_exposure = rospy.get_param('~auto_exposure', 8)
self.auto_exp_prio = rospy.get_param('~auto_exp_prio', 0)
self.white_bal_auto = rospy.get_param('~white_bal_auto', True)
self.white_bal = rospy.get_param('~white_bal', 4600)
# possible values to set can be found by running "rosrun uvc_camera uvc_camera_node _device:=/dev/video0", see http://github.com/ros-drivers/camera_umd.git
dev_list = uvc.device_list()
for i in range(0, len(dev_list)):
print dev_list[i]
rospy.loginfo(
'available device %i: idProd: %i - comparing to idProd: %i'
% (i, int(dev_list[i]["idProduct"]), self.cam_prod_id))
if i == self.cam_prod_id: #int(dev_list[i]["idProduct"]) == self.cam_prod_id:
rospy.loginfo("connecting to camera idProd: %i, device %i" %
(self.cam_prod_id, i))
self.cap = uvc.Capture(dev_list[i]["uid"])
#self.cap.set(CV_CAP_PROP_CONVERT_RGB, false)
rospy.loginfo("successfully connected to camera %i" % i)
rospy.loginfo('starting cam at %ifps with %ix%i resolution' %
(self.fps, self.width, self.height))
self.cap.frame_mode = (self.width, self.height, self.fps)
frame = self.cap.get_frame_robust()
self.controls_dict = dict([(c.display_name, c)
for c in self.cap.controls])
self.controls_dict[
'Brightness'].value = self.brightness #10 #[-64,64], not 0 (no effect)!!
self.controls_dict['Contrast'].value = self.contrast #0 #[0,95]
self.controls_dict['Hue'].value = self.hue #[-2000,2000]
self.controls_dict['Saturation'].value = self.saturation #[0,100]
self.controls_dict['Sharpness'].value = self.sharpness #1 #[1,100]
self.controls_dict['Gamma'].value = self.gamma #[80,300]
self.controls_dict[
'Power Line frequency'].value = self.pwr_line_freq #1:50Hz, 2:60Hz
self.controls_dict[
'Backlight Compensation'].value = self.backlight_comp #True or False
self.controls_dict[
'Absolute Exposure Time'].value = self.exposure_abs #[78,10000] set Auto Exposure Mode to 1
self.controls_dict[
'Auto Exposure Mode'].value = self.auto_exposure #1:manual, 8:apperturePriority
self.controls_dict[
'Auto Exposure Priority'].value = self.auto_exp_prio #1:manual, 8:apperturePriority
self.controls_dict[
'White Balance temperature,Auto'].value = self.white_bal_auto
self.controls_dict[
'White Balance temperature'].value = self.white_bal #[2800,6500]
rospy.loginfo("These camera settings will be applied:")
for c in self.cap.controls:
rospy.loginfo('%s: %i' % (c.display_name, c.value))
self.manager = CameraInfoManager(cname=DEFAULT_CAMERA_NAME,
url='file://' + self.calibration,
namespace=DEFAULT_CAMERA_NAME)
self.manager.loadCameraInfo() # Needs to be called before getter!
self.camera_info = self.manager.getCameraInfo()
self.bridge = CvBridge()
self.image_publisher = rospy.Publisher(self.image_topic,
Image,
queue_size=1)
self.camera_publisher = rospy.Publisher(self.camera_topic,
CameraInfo,
queue_size=1)
self.seq = 0
self.counter = 0
def __init__(self, coz, node_name):
"""
:type coz: cozmo.Robot
:param coz: The cozmo SDK robot handle (object).
:type node_name: String
:param node_name: Name for the node
"""
# initialize node
super().__init__(node_name)
# node timer
self._timer_rate = 10 # Hz
self.timer = self.create_timer(1 / self._timer_rate,
self.timer_callback)
# vars
self._cozmo = coz
self._lin_vel = .0
self._ang_vel = .0
self._cmd_lin_vel = .0
self._cmd_ang_vel = .0
self._last_pose = self._cozmo.pose
self._wheel_vel = (0, 0)
self._optical_frame_orientation = quaternion_from_euler(
-np.pi / 2., .0, -np.pi / 2.)
self._camera_info_manager = CameraInfoManager(
self, 'cozmo_camera', namespace='/cozmo_camera')
# tf
self._tfb = TransformBroadcaster(self)
# params
## TODO: use rosparam when rclpy supports it
#self._odom_frame = rospy.get_param('~odom_frame', 'odom')
self._odom_frame = 'odom'
#self._footprint_frame = rospy.get_param('~footprint_frame', 'base_footprint')
self._footprint_frame = 'base_footprint'
#self._base_frame = rospy.get_param('~base_frame', 'base_link')
self._base_frame = 'base_link'
#self._head_frame = rospy.get_param('~head_frame', 'head_link')
self._head_frame = 'head_link'
#self._camera_frame = rospy.get_param('~camera_frame', 'camera_link')
self._camera_frame = 'camera_link'
#self._camera_optical_frame = rospy.get_param('~camera_optical_frame', 'cozmo_camera')
self._camera_optical_frame = 'cozmo_camera'
#camera_info_url = rospy.get_param('~camera_info_url', '')
camera_info_url = 'file://' + os.path.dirname(
os.path.abspath(__file__)) + '/config/cozmo_camera.yaml'
# pubs
#self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self._joint_state_pub = self.create_publisher(JointState,
'joint_states')
#self._odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self._odom_pub = self.create_publisher(Odometry, 'odom')
#self._imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self._imu_pub = self.create_publisher(Imu, 'imu')
#self._battery_pub = rospy.Publisher('battery', BatteryState, queue_size=1)
self._battery_pub = self.create_publisher(BatteryState, 'battery')
# Note: camera is published under global topic (preceding "/")
#self._image_pub = rospy.Publisher('/cozmo_camera/image', Image, queue_size=10)
self._image_pub = self.create_publisher(Image, '/cozmo_camera/image')
#self._camera_info_pub = rospy.Publisher('/cozmo_camera/camera_info', CameraInfo, queue_size=10)
self._camera_info_pub = self.create_publisher(
CameraInfo, '/cozmo_camera/camera_info')
# subs
#self._backpack_led_sub = rospy.Subscriber(
# 'backpack_led', ColorRGBA, self._set_backpack_led, queue_size=1)
self._backpack_led_sub = self.create_subscription(
ColorRGBA, 'backpack_led', self._set_backpack_led)
#self._twist_sub = rospy.Subscriber('cmd_vel', Twist, self._twist_callback, queue_size=1)
self._twist_sub = self.create_subscription(Twist, 'cmd_vel',
self._twist_callback)
#self._say_sub = rospy.Subscriber('say', String, self._say_callback, queue_size=1)
self._say_sub = self.create_subscription(String, 'say',
self._say_callback)
#self._head_sub = rospy.Subscriber('head_angle', Float64, self._move_head, queue_size=1)
self._head_sub = self.create_subscription(Float64, 'head_angle',
self._move_head)
#self._lift_sub = rospy.Subscriber('lift_height', Float64, self._move_lift, queue_size=1)
self._lift_sub = self.create_subscription(Float64, 'lift_height',
self._move_lift)
# diagnostics
self._diag_array = DiagnosticArray()
self._diag_array.header.frame_id = self._base_frame
diag_status = DiagnosticStatus()
diag_status.hardware_id = 'Cozmo Robot'
diag_status.name = 'Cozmo Status'
diag_status.values.append(KeyValue(key='Battery Voltage', value=''))
diag_status.values.append(KeyValue(key='Head Angle', value=''))
diag_status.values.append(KeyValue(key='Lift Height', value=''))
self._diag_array.status.append(diag_status)
#self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
self._diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics')
# camera info manager
self._camera_info_manager.setURL(camera_info_url)
self._camera_info_manager.loadCameraInfo()
