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 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 load_info(topics):
for t, s in topics.iteritems():
manager = CameraInfoManager(s['name'],s['url'], s['ns'])
manager.loadCameraInfo()
assert( manager.isCalibrated() )
s['info'] = manager.getCameraInfo()
manager.svc.shutdown()
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 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()
class RaspicamRos2(Node):
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 _get_stamp(self, time_float):
time_frac_int = modf(time_float)
stamp = Time()
stamp.sec = int(time_frac_int[1])
stamp.nanosec = int(time_frac_int[0] * 1000000000) & 0xffffffff
return stamp
def publish_image(self, image):
img = Image()
img.encoding = 'rgb8'
img.width = 416
img.height = 320
img.step = img.width * 3
img.data = image
img.header.frame_id = 'raspicam'
img.header.stamp = self._get_stamp(time())
self._img_pub.publish(img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = img.header
self._camera_info_pub.publish(camera_info)
def run(self):
with PiCamera() as camera:
camera.resolution = (416, 320)
# Construct the frame processor and start recording data to it
frame_processor = FrameProcessor(self)
camera.start_recording(frame_processor, 'rgb')
try:
while True:
camera.wait_recording(1)
finally:
camera.stop_recording()
class OpenMVCamNode:
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 read_and_publish_image(self):
# Read image from camera
image = self.openmv_cam.read_image()
# Deduce color/grayscale from image dimensions
channels = 1 if image.ndim == 2 else 3
encoding = 'bgr8' if channels == 3 else 'mono8'
# Convert from BGR to RGB by reversing the channel order
if channels == 3:
image = image[..., ::-1]
# Convert numpy image to ROS image message
image_msg = self.bridge.cv2_to_imgmsg(image, encoding=encoding)
# Add timestamp and sequence number (empty by default)
image_msg.header.stamp = rospy.Time.now()
image_msg.header.seq = self.seq
self.image_publisher.publish(image_msg)
camera_msg = self.camera_info
camera_msg.header = image_msg.header # Copy header from image message
self.camera_publisher.publish(camera_msg)
if self.seq == 0:
rospy.loginfo("Publishing images from OpenMV Cam at '{}' ".format(
self.device))
self.seq += 1
class OpenMVCamNode:
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 read_and_publish_image(self):
# Read image from camera
image = self.openmv_cam.read_image()
# Deduce color/grayscale from image dimensions
channels = 1 if image.ndim == 2 else 3
encoding = 'bgr8' if channels == 3 else 'mono8'
# Convert from BGR to RGB by reversing the channel order
if channels == 3:
image = image[..., ::-1]
# Convert numpy image to ROS image message
image_msg = self.bridge.cv2_to_imgmsg(image, encoding=encoding)
# Add timestamp and sequence number (empty by default)
image_msg.header.stamp = rospy.Time.now()
image_msg.header.seq = self.seq
self.image_publisher.publish(image_msg)
camera_msg = self.camera_info
camera_msg.header = image_msg.header # Copy header from image message
self.camera_publisher.publish(camera_msg)
if self.seq == 0:
rospy.loginfo("Publishing images from OpenMV Cam at '{}' "
.format(self.device))
self.seq += 1
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 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()
class Camera:
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
self._info_manager = self._c0 = self._frame = self._frame_data = None
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 get_camera(self, vimba):
if self._camera_id in vimba._cameras:
rospy.logerr("Requested camera is already in use")
sys.exit(1)
self._c0 = vimba.getCamera(self._camera_id)
self._c0.openCamera()
def gigE_camera(self):
rospy.loginfo("Packet Size: " + str(self._c0.GevSCPSPacketSize))
rospy.loginfo("Stream Bytes Per Second: " + str(self._c0.StreamBytesPerSecond))
self._c0.runFeatureCommand("GVSPAdjustPacketSize")
self._c0.StreamBytesPerSecond = 100000000
def set_pixel_format(self):
self._c0.PixelFormat = "RGB8Packed"
self._c0.AcquisitionMode = "Continuous"
self._c0.ExposureAuto = "Continuous"
self._c0.Width = 1210
self._c0.Height = 760
self._frame = self._c0.getFrame()
self._frame.announceFrame()
def initialize_camera(self, vimba):
self.find_camera(vimba)
self.get_camera(vimba)
try:
# gigE camera
self.gigE_camera()
except Exception:
# not a gigE camera
pass
self.set_pixel_format()
def start_capture(self):
self._c0.startCapture()
def start_acquisition(self):
self._c0.runFeatureCommand("AcquisitionStart")
def queue_frame_capture(self):
self._frame.queueFrameCapture()
self._frame.waitFrameCapture()
def get_frame_data(self):
self._frame_data = self._frame.getBufferByteData()
def publish_image(self, time):
img = np.ndarray(buffer=self._frame_data,
dtype=np.uint8,
shape=(self._frame.height, self._frame.width, self._frame.pixel_bytes))
img_message = self._bridge.cv2_to_imgmsg(img, "rgb8")
img_message.header.stamp = time
self._img_pub.publish(img_message)
self._info_pub.publish(self._info_manager.getCameraInfo())
def stop_acquisition(self):
self._c0.runFeatureCommand("AcquisitionStop")
self._c0.endCapture()
self._c0.revokeAllFrames()
self._c0.closeCamera()
class NaoCam (NaoNode):
def __init__(self):
NaoNode.__init__(self)
rospy.init_node('nao_camera')
self.camProxy = self.getProxy("ALVideoDevice")
if self.camProxy is None:
exit(1)
# ROS pub/sub
self.pub_img_ = []
self.pub_info_ = []
for i in ['/nao_cam/top/','/nao_cam/bottom/']:
self.pub_img_ += [rospy.Publisher(i+'image_raw', Image)]
self.pub_info_ += [rospy.Publisher(i+'camera_info', CameraInfo)]
# self.set_camera_info_service_ = rospy.Service('set_camera_info', SetCameraInfo, self.set_camera_info)
topCamParamsURL = rospy.get_param('~topCamParams','')
bottomCamParamsURL = rospy.get_param('~bottomCamParams','')
print "urls", topCamParamsURL, bottomCamParamsURL
self.tCIM = CameraInfoManager('/nao_cam/top/','narrow_stereo/top', topCamParamsURL)
self.bCIM = CameraInfoManager('/nao_cam/bottom/','narrow_stereo/bottom', bottomCamParamsURL)
self.tCIM.loadCameraInfo()
self.bCIM.loadCameraInfo()
# Messages
# self.info_ = CameraInfo()
# self.set_default_params_qvga(self.info_)
self.info_ = []
self.info_.append(self.tCIM.getCameraInfo())
self.info_.append(self.bCIM.getCameraInfo())
#TODO: parameters
self.resolution = rospy.get_param('~quality',30)
self.colorSpace = kBGRColorSpace
self.fps = rospy.get_param('~fps',20)
print "using fps: ", self.fps
# init
self.nameId = "rospy_gvm"
self.subscribe()
def subscribe(self):
# subscribe to both cameras
rospy.loginfo("Suscribing to cameras")
try:
self.nameId = self.camProxy.subscribeCameras(self.nameId,[0,1], [self.resolution,self.resolution],[self.colorSpace,self.colorSpace],self.fps)
except:
rospy.loginfo("Suscribing did not work, unsuscribing and trying again ")
self.camProxy.unsubscribeAllInstances(self.nameId)
self.nameId = self.camProxy.subscribeCameras(self.nameId,[0,1], [self.resolution,self.resolution],[self.colorSpace,self.colorSpace],self.fps)
print "subscribed"
def set_camera_info(self, cameraInfoMsg):
# print "Received new camera info"
self.info_ = cameraInfoMsg.camera_info
def set_default_params_qvga(self, cam_info):
cam_info.P[0] = 268.663727
cam_info.P[1] = 0.0
cam_info.P[2] = 179.971761
cam_info.P[3] = 0
cam_info.P[4] = 0.0
cam_info.P[5] = 271.859741
cam_info.P[6] = 117.122489
cam_info.P[7] = 0.0
cam_info.P[8] = 0.0
cam_info.P[9] = 0.0
cam_info.P[10] = 1.0
cam_info.P[11] = 0.0
cam_info.D = [-0.064567]
def main_loop(self):
img = Image()
while not rospy.is_shutdown():
#print "getting image..",
images = self.camProxy.getImagesRemote (self.nameId)
#print "ok"
# TODO: better time
for i in [0,1]:
#print len(images[i])
image = images[i]
img.header.stamp = rospy.Time.now()
if image[7] == 0:
img.header.frame_id = "/CameraTop_frame"
elif image[7] == 1:
img.header.frame_id = "/CameraBottom_frame"
img.height = image[1]
img.width = image[0]
nbLayers = image[2]
#colorspace = image[3]
if image[3] == kYUVColorSpace:
encoding = "mono8"
elif image[3] == kRGBColorSpace:
encoding = "rgb8"
elif image[3] == kBGRColorSpace:
encoding = "bgr8"
else:
rospy.logerror("Received unknown encoding: {0}".format(image[3]))
img.encoding = encoding
img.step = img.width * nbLayers
if len(images) >= 2:
img.data = images[2][len(images[2])/2 * i:len(images[2])/2 *(i+1) + 1]
else:
img.data = []
print "image with no data"
self.info_[i].width = img.width
self.info_[i].height = img.height
self.info_[i].header = img.header
self.pub_img_[i].publish(img)
self.pub_info_[i].publish(self.info_[i])
self.camProxy.releaseImages(self.nameId)
self.camProxy.unsubscribe(self.nameId)
if __name__ == '__main__':
rospy.init_node('camera')
cap = cv2.VideoCapture()
cap.open(rospy.get_param('~camera_id', 0))
manager = CameraInfoManager(rospy.get_name().strip('/'))
manager.loadCameraInfo()
image_pub = rospy.Publisher('~image_raw', Image)
info_pub = rospy.Publisher('~camera_info', CameraInfo)
rate = rospy.Rate(rospy.get_param('~publish_rate', 15))
header = rospy.Header()
header.frame_id = rospy.get_name() + '_rgb_optical_frame'
while not rospy.is_shutdown():
cap.grab()
frame = cap.retrieve()
header.stamp = rospy.Time.now()
if frame[0]:
msg = _bridge.cv_to_imgmsg(cv2.cv.fromarray(frame[1]), "bgr8")
msg.header = header
if manager.isCalibrated():
info = manager.getCameraInfo()
info.header = header
info_pub.publish(info)
image_pub.publish(msg)
rate.sleep()
cap.release()
class Gopro:
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 stream(self):
rospy.loginfo("Requesting video stream from solo camera...")
try:
socket.setdefaulttimeout(5)
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
try:
self.socket.connect(('10.1.1.1', 5502))
rospy.loginfo("Connected to solo camera")
self.stream_frames()
except socket.timeout:
rospy.logerr("Timed out connecting to solo camera")
except socket.error as msg:
rospy.logerr("Socket error: %s" % msg)
finally:
if self.socket is not None:
self.socket.close()
self.socket = None
rospy.logdebug("Disconnected from solo camera")
def stream_frames(self):
rospy.loginfo("Opening video stream")
stream = cv2.VideoCapture(self.sololink_config)
running = True
bad_frames = 0
while (stream.isOpened() and running):
frame_read_stat, cv2_image = stream.read()
if frame_read_stat:
# Reset the bad frame counter since we got a good frames
if (bad_frames > 0):
rospy.logerror("Recovering from %d dropped frames" %
bad_frames)
bad_frames = 0
# Publish the captured image
try:
ros_image = CvBridge().cv2_to_imgmsg(cv2_image, 'bgr8')
stamp = rospy.Time.now()
ros_image.header.stamp = stamp
self.image_raw_publisher.publish(ros_image)
self.camera_info.header.stamp = stamp
self.camera_info_publisher.publish(self.camera_info)
except TypeError as e:
rospy.logerr("CvBridge could not convert image: %s", e)
else:
#rospy.logerr("Could not capture video frame (%d unreadable frames)" % bad_frames)
bad_frames += 1
if bad_frames > MAX_BAD_FRAMES:
rospy.logerr(
"Closing stream after encountering %d unreadable frames" %
MAX_BAD_FRAMES)
running = False
else:
rospy.logerr("Video stream is now closed")
class CozmoRos(object):
"""
The Cozmo ROS driver object.
"""
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 _publish_diagnostics(self):
# alias
diag_status = self._diag_array.status[0]
# fill diagnostics array
battery_voltage = self._cozmo.battery_voltage
diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
diag_status.values[1].value = '{:.2f} deg'.format(
self._cozmo.head_angle.degrees)
diag_status.values[2].value = '{:.2f} mm'.format(
self._cozmo.lift_height.distance_mm)
if battery_voltage > 3.5:
diag_status.level = DiagnosticStatus.OK
diag_status.message = 'Everything OK!'
elif battery_voltage > 3.4:
diag_status.level = DiagnosticStatus.WARN
diag_status.message = 'Battery low! Go charge soon!'
else:
diag_status.level = DiagnosticStatus.ERROR
diag_status.message = 'Battery very low! Cozmo will power off soon!'
# update message stamp and publish
self._diag_array.header.stamp = rospy.Time.now()
self._diag_pub.publish(self._diag_array)
def _move_head(self, cmd):
"""
Move head to given angle.
:type cmd: Float64
:param cmd: The message containing angle in degrees. [-25 - 44.5]
"""
self.move_head(cmd.data)
def move_head(self, angle):
angle = cozmo.util.radians(angle * np.pi / 180.)
action = self._cozmo.set_head_angle(angle,
duration=0.1,
in_parallel=True)
action.wait_for_completed()
def _move_lift(self, cmd):
"""
Move lift to given height.
:type cmd: Float64
:param cmd: A value between [0 - 1], the SDK auto
scales it to the according height.
"""
self.move_lift(cmd.data)
def move_lift(self, level, duration=0.2):
action = self._cozmo.set_lift_height(height=level,
duration=0.2,
in_parallel=True)
action.wait_for_completed()
def _set_backpack_led(self, msg):
"""
Set the color of the backpack LEDs.
:type msg: ColorRGBA
:param msg: The color to be set.
"""
# setup color as integer values
color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
# create lights object with duration
light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
on_period_ms=1000)
# set lights
self._cozmo.set_all_backpack_lights(light)
def _twist_callback(self, cmd):
"""
Set commanded velocities from Twist message.
The commands are actually send/set during run loop, so delay
is in worst case up to 1 / update_rate seconds.
:type cmd: Twist
:param cmd: The commanded velocities.
"""
self.set_velocity(cmd.linear.x, cmd.angular.z)
def set_velocity(self, linear, angular):
# compute differential wheel speed
axle_length = 0.07 # 7cm
self._cmd_lin_vel = linear
self._cmd_ang_vel = angular
rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
self._wheel_vel = (lv * 1000., rv * 1000.) # convert to mm / s
def _say_callback(self, msg):
"""
The callback for incoming text messages to be said.
:type msg: String
:param msg: The text message to say.
"""
self._cozmo.say_text(msg.data).wait_for_completed()
def _publish_objects(self):
"""
Publish detected object as transforms between odom_frame and object_frame.
"""
for obj in self._cozmo.world.visible_objects:
now = rospy.Time.now()
x = obj.pose.position.x * 0.001
y = obj.pose.position.y * 0.001
z = obj.pose.position.z * 0.001
q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
obj.pose.rotation.q3, obj.pose.rotation.q0)
self._tfb.send_transform((x, y, z), q, now,
'cube_' + str(obj.object_id),
self._odom_frame)
try:
if obj.cube_id and self.target_cube != obj:
self._tfb.send_transform((x, y, z), q, now,
'cube_' + str(obj.object_id),
self._odom_frame)
print("Found {}".format(obj.cube_id))
if not self.cube_found and self.robots_distance_to_object(
self._cozmo, obj) < 400:
self.target_cube = obj
self.cube_found = True
print("Locking on to {}".format(obj.cube_id))
else:
if self.cube_found:
print("Found that one already!")
else:
print("Cube too far away!")
except:
# print('OBJECT IS NOT A LIGHT CUBE')
if (obj == self._cozmo.world.charger):
return
if (obj.object_type == CustomObjectTypes.CustomType00
and (self.front_wall_pose == None
or not self.front_wall_pose.is_accurate)):
self.front_wall_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Front',
self._odom_frame)
print('*** Comzmo has found the front wall! ***')
if (obj.object_type == CustomObjectTypes.CustomType01
and (self.ramp_bottom_pose == None
or not self.ramp_bottom_pose.is_accurate)):
self.ramp_bottom_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Ramp',
self._odom_frame)
print('*** Comzmo has found the front wall! ***')
if (obj.object_type == CustomObjectTypes.CustomType03
and (self.drop_spot_pose == None
or not self.drop_spot_pose.is_accurate)):
self.drop_spot_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Drop',
self._odom_frame)
print('*** Comzmo has found the drop Spot! ***')
if (obj.object_type == CustomObjectTypes.CustomType04
and (self.back_wall_pose == None
or not self.back_wall_pose.is_accurate)):
self.back_wall_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Back',
self._odom_frame)
print('*** Comzmo has found the back wall! ***')
if (obj.object_type == CustomObjectTypes.CustomType05
and (self.drop_target_pose == None
or not self.drop_target_pose.is_accurate)):
self.drop_target_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Target',
self._odom_frame)
print('*** Comzmo has found the Dropt Target! ***')
if (obj.object_type == CustomObjectTypes.CustomType06
and (self.drop_clue_pose == None
or not self.drop_clue_pose.is_accurate)):
self.drop_clue_pose = obj.pose
self._tfb.send_transform((x, y, z), q, now, 'Clue',
self._odom_frame)
print('*** Comzmo has found the Dropt Clue! ***')
def _publish_image(self):
"""
Publish latest camera image as Image with CameraInfo.
"""
# only publish if we have a subscriber
if self._image_pub.get_num_connections() == 0:
return
# get latest image from cozmo's camera
camera_image = self._cozmo.world.latest_image
if camera_image is not None:
# convert image to gray scale as it is gray although
img = camera_image.raw_image.convert('L')
ros_img = Image()
ros_img.encoding = 'mono8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = ros_img.width
ros_img.data = img.tobytes()
ros_img.header.frame_id = 'cozmo_camera'
cozmo_time = camera_image.image_recv_time
ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
# publish images and camera info
self._image_pub.publish(ros_img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = ros_img.header
self._camera_info_pub.publish(camera_info)
def _publish_joint_state(self):
"""
Publish joint states as JointStates.
"""
# only publish if we have a subscriber
if self._joint_state_pub.get_num_connections() == 0:
return
js = JointState()
js.header.stamp = rospy.Time.now()
js.header.frame_id = 'cozmo'
js.name = ['head', 'lift']
js.position = [
self._cozmo.head_angle.radians,
self._cozmo.lift_height.distance_mm * 0.001
]
js.velocity = [0.0, 0.0]
js.effort = [0.0, 0.0]
self._joint_state_pub.publish(js)
def _publish_imu(self):
"""
Publish inertia data as Imu message.
"""
# only publish if we have a subscriber
if self._imu_pub.get_num_connections() == 0:
return
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self._base_frame
imu.orientation.w = self._cozmo.pose.rotation.q0
imu.orientation.x = self._cozmo.pose.rotation.q1
imu.orientation.y = self._cozmo.pose.rotation.q2
imu.orientation.z = self._cozmo.pose.rotation.q3
imu.angular_velocity.x = self._cozmo.gyro.x
imu.angular_velocity.y = self._cozmo.gyro.y
imu.angular_velocity.z = self._cozmo.gyro.z
imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
self._imu_pub.publish(imu)
def _publish_battery(self):
"""
Publish battery as BatteryState message.
"""
# only publish if we have a subscriber
if self._battery_pub.get_num_connections() == 0:
return
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
def _publish_odometry(self):
"""
Publish current pose as Odometry message.
"""
# only publish if we have a subscriber
if self._odom_pub.get_num_connections() == 0:
return
now = rospy.Time.now()
odom = Odometry()
odom.header.frame_id = self._odom_frame
odom.header.stamp = now
odom.child_frame_id = self._footprint_frame
odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
1e-1]).ravel()
odom.twist.twist.linear.x = self._lin_vel
odom.twist.twist.angular.z = self._ang_vel
odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
1e-2]).ravel()
self._odom_pub.publish(odom)
def _publish_tf(self, update_rate):
"""
Broadcast current transformations and update
measured velocities for odometry twist.
Published transforms:
odom_frame -> footprint_frame
footprint_frame -> base_frame
base_frame -> head_frame
head_frame -> camera_frame
camera_frame -> camera_optical_frame
"""
now = rospy.Time.now()
x = self._cozmo.pose.position.x * 0.001
y = self._cozmo.pose.position.y * 0.001
z = self._cozmo.pose.position.z * 0.001
# compute current linear and angular velocity from pose change
# Note: Sign for linear velocity is taken from commanded velocities!
# Note: The angular velocity can also be taken from gyroscopes!
delta_pose = self._last_pose - self._cozmo.pose
dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
delta_pose.position.z**2) / 1000.0
self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate
# publish odom_frame -> footprint_frame
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
self._odom_frame)
# publish footprint_frame -> base_frame
q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
self._footprint_frame)
# publish base_frame -> head_frame
q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
self._base_frame)
# publish head_frame -> camera_frame
self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
now, self._camera_frame, self._head_frame)
# publish camera_frame -> camera_optical_frame
q = self._optical_frame_orientation
self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
self._camera_optical_frame,
self._camera_frame)
# store last pose
self._last_pose = deepcopy(self._cozmo.pose)
def run(self, update_rate=10):
"""
Publish data continuously with given rate.
:type update_rate: int
:param update_rate: The update rate.
"""
r = rospy.Rate(update_rate)
while not rospy.is_shutdown():
self._publish_tf(update_rate)
self._publish_image()
self._publish_objects()
self._publish_joint_state()
self._publish_imu()
self._publish_battery()
self._publish_odometry()
self._publish_diagnostics()
# send message repeatedly to avoid idle mode.
# This might cause low battery soon
# TODO improve this!
if self.is_up_top:
self.move_cubes()
else:
self.climb_ramp()
self._cozmo.drive_wheels(*self._wheel_vel)
# sleep
r.sleep()
# stop events on cozmo
self._cozmo.stop_all_motors()
def move_cubes(self):
if self.action != None and self.action.is_running:
return
if self.task == 0: # move to a start location
if (self.drop_spot_pose == None
or not self.drop_spot_pose.is_accurate):
self.move_head(0)
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(10),
in_parallel=True)
print("Oops! Let me find where I am.")
pass
else:
#_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
#print(_loc)
offset = cozmo.util.Pose(-120, 25, 0, 0, 0, 0, 0)
offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.cube_found = False
self.task = 1
print("*" * 5 + " Moved to start position! " + "*" * 5)
elif self.task == 1:
if (not self.cube_found):
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
in_parallel=True)
print("Looking for a cube...")
else:
self.action = self._cozmo.pickup_object(
self.target_cube, True, False, 2)
print("Picking up a cube!")
self.task = 2
elif self.task == 2:
if (self.drop_spot_pose == None
or not self.drop_spot_pose.is_accurate):
self.move_head(0)
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
in_parallel=True)
print("Oops! Let me find where I am.")
pass
else:
offset = cozmo.util.Pose(-150, 40, 0, 0, 0, 0, 0)
offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.task = 3
print("*" * 5 + " Move to relocalize! " + "*" * 5)
elif self.task == 3:
offset = cozmo.util.Pose(-100, 40, 0, 0, 0, 0, 0)
offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
#self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
self.task = 4
print("*" * 5 + " Moving toward drop position! " + "*" * 5)
elif self.task == 4:
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(-90),
in_parallel=True)
print("Turning into position...")
self.task = 5
elif self.task == 5:
#self.action = self._cozmo.place_object_on_ground_here(self.target_cube)
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(-30),
cozmo.util.speed_mmps(10),
in_parallel=True)
self.task = 6
elif self.task == 6:
if (self.drop_clue_pose == None
or not self.drop_clue_pose.is_accurate):
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(-10),
cozmo.util.speed_mmps(10),
in_parallel=True)
print("Oops! Let me find the clue.")
pass
else:
offset = cozmo.util.Pose(-50, -60, 0, 0, 0, 0, 0)
offset1 = self.drop_clue_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
#self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
self.task = 7
print("*" * 5 + " Moving to find drop position! " + "*" * 5)
elif self.task == 7:
if (self.drop_target_pose == None
or not self.drop_target_pose.is_accurate):
self.move_head(-25)
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(-10),
cozmo.util.speed_mmps(10),
in_parallel=True)
print("Oops! Let me find the target.")
pass
else:
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(15),
cozmo.util.speed_mmps(10),
in_parallel=True)
#offset = cozmo.util.Pose(0, 0, -40, 0, 0, 0, 0)
#offset1 =self.drop_target_pose.define_pose_relative_this(offset)
#self.action=self._cozmo.go_to_pose(offset1)
#self.action = self._cozmo.drive_straight(cozmo.util.distance_mm(40), cozmo.util.speed_mmps(10), in_parallel=True)
self.task = 8
print("*" * 5 + " Moving to drop position! " + "*" * 5)
elif self.task == 8:
#self.action = self._cozmo.place_object_on_ground_here(self.target_cube)
self.move_lift(0, 1.0)
self.move_head(0)
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(-30),
cozmo.util.speed_mmps(10),
in_parallel=True)
self.target_cube = None # Just placed it, back away
self.cube_found = False # Forget about it
self.task = 9
print("*" * 5 + " Placed Cube! " + "*" * 5)
elif self.task == 9:
if (self.back_wall_pose == None
or not self.back_wall_pose.is_accurate):
self.move_head(10)
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(20),
in_parallel=True)
print("Oops! Let me find where I am.")
else:
offset = cozmo.util.Pose(-150, 40, 0, 0, 0, 0, 0)
offset1 = self.back_wall_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.task = 10
print("*" * 5 + " Move to relocalize! " + "*" * 5)
elif self.task == 10:
if (self.drop_spot_pose == None
or not self.drop_spot_pose.is_accurate):
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(10),
in_parallel=True)
print("*" * 5 + " Relocalizing... " + "*" * 5)
else:
self.task = 0
self.back_wall_pose.invalidate()
self.drop_clue_pose.invalidate()
self.drop_target_pose.invalidate()
self.drop_spot_pose.invalidate()
def climb_ramp(self):
if self.action != None and self.action.is_running:
return
if self.task == 0: # set goal
d_x = 50
d_y = 50
d_angle = cozmo.util.degrees(0)
self.goal_pose = cozmo.util.pose_z_angle(d_x, d_y, 0, d_angle, 0)
self.action = self._cozmo.go_to_pose(self.goal_pose,
relative_to_robot=True,
in_parallel=True,
num_retries=2)
self.task = 1
print("*" * 10 + " Step 1 Done! " + "*" * 10)
elif self.task == 1 and (self.front_wall_pose != None
and self.front_wall_pose.is_accurate):
#_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
#print(_loc)
offset = cozmo.util.Pose(-150, 0, 0, 0, 0, 0, 0)
offset1 = self.front_wall_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.task = 2
print("*" * 10 + " Step 2 Done! " + "*" * 10)
elif self.task == 2: # set goal
d_x = 0
d_y = 300
d_angle = cozmo.util.degrees(-90)
self.goal_pose = cozmo.util.pose_z_angle(d_x, d_y, 0, d_angle, 0)
self.action = self._cozmo.go_to_pose(self.goal_pose,
relative_to_robot=True,
in_parallel=True,
num_retries=2)
self.task = 3
print("*" * 10 + " Step 3 Done! " + "*" * 10)
elif self.task == 3 and (self.ramp_bottom_pose != None
and self.ramp_bottom_pose.is_accurate):
#_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
#print(_loc)
offset = cozmo.util.Pose(-45, 40, 0, 0, 0, 0, 0)
offset1 = self.ramp_bottom_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.task = 4
print("*" * 10 + " Step 4 Done! " + "*" * 10)
elif self.task == 4: # move forward
self.action = self._cozmo.turn_in_place(cozmo.util.degrees(-90),
in_parallel=True)
self.task = 5
print("*" * 10 + " Step 5 Done! " + "*" * 10)
elif self.task == 5:
print("*" * 10 + " At the ramp! " + "*" * 10)
self.action = self._cozmo.drive_straight(
cozmo.util.distance_mm(200),
cozmo.util.speed_mmps(10),
in_parallel=True)
self.task = 6
print("*" * 10 + " Step 6 Done! " + "*" * 10)
elif self.task == 6 and (self.drop_spot_pose != None
and self.drop_spot_pose.is_accurate):
#_loc=self.pose_with_distance_from_target(self._cozmo,self.front_wall,50)
#print(_loc)
offset = cozmo.util.Pose(-200, 20, 0, 0, 0, 0, 0)
offset1 = self.drop_spot_pose.define_pose_relative_this(offset)
self.action = self._cozmo.go_to_pose(offset1)
self.task = 7
print("*" * 10 + " Step 7 Done! " + "*" * 10)
elif self.task == 7:
self.task = 0 # Reset the tasks
self.is_up_top = True # Cozmo made it up
print("*" * 10 + " Made it to the top! " + "*" * 10)
def handle_object_appeared(self, evt, **kw):
# This will be called whenever an EvtObjectAppeared is dispatched -
# whenever an Object comes into view.
try:
print("*** Cozmo started seeing a %s" % str(evt.obj.object_type))
if isinstance(evt.obj, CustomObject):
print("*** Cozmo started seeing a %s" %
str(evt.obj.object_type))
except:
print("*** Cozmo found a cube?")
def handle_object_disappeared(self, evt, **kw):
# This will be called whenever an EvtObjectDisappeared is dispatched -
# whenever an Object goes out of view.
try:
print("!!! Cozmo stopped seeing a %s" % str(evt.obj.object_type))
if isinstance(evt.obj, CustomObject):
print("!!! Cozmo stopped seeing a %s" %
str(evt.obj.object_type))
except:
print("*** Cozmo lost sight of a cube?")
def robots_distance_to_object(self, robot, target):
object_vector = np.array(
(target.pose.position.x - robot.pose.position.x,
target.pose.position.y - robot.pose.position.y))
dist = np.sqrt((object_vector**2).sum())
return dist
def pose_with_distance_from_target(self, robot, target, dist):
dist_scalar = (dist / self.robots_distance_to_object(robot, target))
dest_pos = target.pose.position + cozmo.util.Vector3(
(robot.pose.position.x - target.pose.position.x) * dist_scalar,
(robot.pose.position.y - target.pose.position.y) * dist_scalar,
0.0)
return dest_pos
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...")
class CozmoRos(object):
"""
The Cozmo ROS driver object.
"""
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')
self.loc1Found = False
self.loc2Found = False
self.loc3Found = False
self.rampFound = False
self.allLocFound = False
self.topBackWallFound = False
self.frontOfBoxFound = False
self.topSideWallFound = False
self.readyToPickUp = False
self.deliveredCube1 = None
self.deliveredCube2 = None
self.deliveredCube3 = None
# 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()
self.twist = Twist()
self.custom_objects()
#self.image_sub = rospy.Subscriber('camera/rgb/image_raw',
# Image, self.image_callback)
def custom_objects(self):
# Add event handlers for whenever Cozmo sees a new object
#robot.add_event_handler(cozmo.objects.EvtObjectAppeared, handle_object_appeared)
#robot.add_event_handler(cozmo.objects.EvtObjectDisappeared, handle_object_disappeared)
self.targetLocation1 = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType00, CustomObjectMarkers.Hexagons3, 5,
63, 63, True)
self.targetLocation2 = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType01, CustomObjectMarkers.Triangles2, 5,
63, 63, True)
self.targetLocation3 = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles2, 5,
63, 63, True)
self.ramp = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType03, CustomObjectMarkers.Diamonds2, 1,
45, 45, True)
self.topBackWall = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType04, CustomObjectMarkers.Diamonds4, 1,
45, 45, True)
self.frontOfBox = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType05, CustomObjectMarkers.Hexagons2, 1,
63, 63, True)
self.topSideWall = self._cozmo.world.define_custom_cube(
CustomObjectTypes.CustomType06, CustomObjectMarkers.Circles4, 1,
45, 45, True)
if ((self.targetLocation1 is not None)
and (self.targetLocation2 is not None)
and (self.targetLocation3 is not None)
and (self.ramp is not None)):
print("All objects defined successfully!")
else:
print("One or more object definitions failed!")
return
def _publish_diagnostics(self):
# alias
diag_status = self._diag_array.status[0]
# fill diagnostics array
battery_voltage = self._cozmo.battery_voltage
diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
diag_status.values[1].value = '{:.2f} deg'.format(
self._cozmo.head_angle.degrees)
diag_status.values[2].value = '{:.2f} mm'.format(
self._cozmo.lift_height.distance_mm)
if battery_voltage > 3.5:
diag_status.level = DiagnosticStatus.OK
diag_status.message = 'Everything OK!'
elif battery_voltage > 3.4:
diag_status.level = DiagnosticStatus.WARN
diag_status.message = 'Battery low! Go charge soon!'
else:
diag_status.level = DiagnosticStatus.ERROR
diag_status.message = 'Battery very low! Cozmo will power off soon!'
# update message stamp and publish
self._diag_array.header.stamp = rospy.Time.now()
self._diag_pub.publish(self._diag_array)
"""
Move lift to given height.
"""
def _resetHead(self):
action1 = self._cozmo.set_head_angle(radians(0.20),
duration=0.1,
in_parallel=True)
action1.wait_for_completed()
action2 = self._cozmo.set_lift_height(0,
duration=0.2,
in_parallel=True)
action2.wait_for_completed()
def _move_head(self, cmd):
"""
Move head to given angle.
:type cmd: Float64
:param cmd: The message containing angle in degrees. [-25 - 44.5]
"""
action = self._cozmo.set_head_angle(radians(cmd.data * np.pi / 180.),
duration=0.1,
in_parallel=True)
action.wait_for_completed()
def _move_lift(self, cmd):
"""
Move lift to given height.
:type cmd: Float64
:param cmd: A value between [0 - 1], the SDK auto
scales it to the according height.
"""
action = self._cozmo.set_lift_height(height=cmd.data,
duration=0.2,
in_parallel=True)
action.wait_for_completed()
def _set_backpack_led(self, msg):
"""
Set the color of the backpack LEDs.
:type msg: ColorRGBA
:param msg: The color to be set.
"""
# setup color as integer values
color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
# create lights object with duration
light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
on_period_ms=1000)
# set lights
self._cozmo.set_all_backpack_lights(light)
def _twist_callback(self, cmd):
"""
Set commanded velocities from Twist message.
The commands are actually send/set during run loop, so delay
is in worst case up to 1 / update_rate seconds.
:type cmd: Twist
:param cmd: The commanded velocities.
"""
# compute differential wheel speed
axle_length = 0.07 # 7cm
self._cmd_lin_vel = cmd.linear.x
self._cmd_ang_vel = cmd.angular.z
rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
self._wheel_vel = (lv * 1000., rv * 1000.) # convert to mm / s
def _say_callback(self, msg):
"""
The callback for incoming text messages to be said.
:type msg: String
:param msg: The text message to say.
"""
self._cozmo.say_text(msg.data).wait_for_completed()
def robots_distance_to_object(self, robot, target):
object_vector = np.array(
(target.pose.position.x - robot.pose.position.x,
target.pose.position.y - robot.pose.position.y))
dist = np.sqrt((object_vector**2).sum())
return dist
def pose_with_distance_from_target(self, robot, target, dist):
dist_scalar = (dist / self.robots_distance_to_object(robot, target))
dest_pos = target.pose.position + cozmo.util.Vector3(
(robot.pose.position.x - target.pose.position.x) * dist_scalar,
(robot.pose.position.y - target.pose.position.y) * dist_scalar,
0.0)
return dest_pos
def _publish_objects(self):
"""
Publish detected object as transforms between odom_frame and object_frame.
"""
for obj in self._cozmo.world.visible_objects:
now = rospy.Time.now()
x = obj.pose.position.x * 0.001
y = obj.pose.position.y * 0.001
z = obj.pose.position.z * 0.001
q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
obj.pose.rotation.q3, obj.pose.rotation.q0)
#self._cozmo.go_to_pose(Pose(obj.pose.x/2,obj.pose.y/2,obj.pose.z/2,obj.pose.rotation.q1, obj.pose.rotation.q2, obj.pose.rotation.q3, obj.pose.rotation.q0)).wait_for_completed()
try:
if (obj.cube_id):
self._tfb.send_transform((x, y, z), q, now,
'cube_' + str(obj.object_id),
self._odom_frame)
# print('found cube '+str(obj.cube_id))
if (self.allLocFound
and self.readyToPickUp): #self.allLocFound
num = obj.cube_id
# if(not self._checkDistToLoc(num,obj)):
# print('cube '+ str(obj.cube_id)+ 'has already been delivered')
# return
if (not self._checkDistToSelf(obj)):
print("cube too far")
continue
self.action = self._cozmo.pickup_object(
obj, True, False, 2
) #dock_with_cube(obj, approach_angle=cozmo.util.degrees(0), num_retries=2)
self.action.wait_for_completed()
if (self.action.has_succeeded):
if (num == 1):
if (self.loc1Pose.is_accurate):
#
if (self.deliveredCube1 == None):
offset = cozmo.util.Pose(
-100, 50, 0, 0, 0, 0, 0)
offset1 = self.targetLocation1.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_completed()
self.readyToPickUp = False
self.deliveredCube1 = obj
else:
offset = cozmo.util.Pose(
-100, 0, 0, 0, 0, 0, 0)
offset1 = self.targetLocation1.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_completed()
print("delivered 2nd cube")
self.readyToPickUp = False
else:
self.action2 = self._cozmo.set_lift_height(
0,
duration=0.5,
in_parallel=True)
self.action2.wait_for_completed()
self.readyToPickUp = False
else:
print(
"loc1 is no longer accurate, need to relocate"
)
self.action = self._cozmo.go_to_pose(
cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
self.action.wait_for_completed()
self.allLocFound = False
self.frontOfBoxFound = False
self.loc1Found = False
self.loc2Found = False
self.loc3Found = False
self._resetHead()
elif (num == 2):
if (self.loc2Pose.is_accurate):
#
if (self.deliveredCube2 == None):
offset = cozmo.util.Pose(
-100, -50, 0, 0, 0, 0, 0)
offset1 = self.targetLocation2.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_completed()
self.readyToPickUp = False
self.deliveredCube2 = obj
else:
offset = cozmo.util.Pose(
-100, 50, 0, 0, 0, 0, 0)
offset1 = self.targetLocation2.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_complete()
print("delivered 2nd cube")
else:
self.action2 = self._cozmo.set_lift_height(
0,
duration=0.5,
in_parallel=True)
self.action2.wait_for_completed()
self.readyToPickUp = False
else:
print(
"the side wall is no longer accurate, need to relocate"
)
self.action = self._cozmo.go_to_pose(
cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
self.action.wait_for_completed()
self.allLocFound = False
self.frontOfBoxFound = False
self.loc1Found = False
self.loc2Found = False
self.loc3Found = False
self._resetHead()
elif (num == 3):
if (self.loc3Pose.is_accurate):
#
if (self.deliveredCube3 == None):
offset = cozmo.util.Pose(
-120, -70, 0, 0, 0, 0, 0)
offset1 = self.targetLocation3.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_completed()
self.readyToPickUp = False
self.deliveredCube3 = obj
else:
offset = cozmo.util.Pose(
-120, 70, 0, 0, 0, 0, 0)
offset1 = self.targetLocation3.pose.define_pose_relative_this(
offset)
self.action = self._cozmo.go_to_pose(
offset1)
self.action.wait_for_completed()
if (self.action.has_succeeded):
self.action = self._cozmo.place_object_on_ground_here(
obj)
self.action.wait_for_completed()
self.readyToPickUp = False
print("delivered 2nd cube")
else:
self.action2 = self._cozmo.set_lift_height(
0,
duration=0.5,
in_parallel=True)
self.action2.wait_for_completed()
self.readyToPickUp = False
else:
print(
"the side wall is no longer accurate, need to relocate"
)
self.action = self._cozmo.go_to_pose(
cozmo.util.Pose(0, 0, 0, 0, 0, 0, 0))
self.action.wait_for_completed()
self.allLocFound = False
self.frontOfBoxFound = False
self.loc1Found = False
self.loc2Found = False
self.loc3Found = False
self._resetHead()
print("result:", self.action.result)
else:
self.readyToPickUp = False
except:
# print('OBJECT IS NOT A LIGHT CUBE')
if (obj == self._cozmo.world.charger):
continue
if (obj.object_type == CustomObjectTypes.CustomType03
and (not self.rampFound)):
self.rampPose = obj.pose
self.rampFound = True
self._tfb.send_transform((x, y, z), q, now, 'Ramp',
self._odom_frame)
print('comzmo has found ramp')
elif (obj.object_type == CustomObjectTypes.CustomType00
and (not self.loc1Found)):
self.loc1Pose = obj.pose
self.targetLocation1 = obj
self.loc1Found = True
self._tfb.send_transform((x, y, z), q, now, 'Endpoint 1',
self._odom_frame)
print('comzmo has found loc1')
elif (obj.object_type == CustomObjectTypes.CustomType01
and (not self.loc2Found)):
self.loc2Pose = obj.pose
self.loc2Found = True
self.targetLocation2 = obj
self._tfb.send_transform((x, y, z), q, now, 'Endpoint 2',
self._odom_frame)
print('comzmo has found loc2')
elif (obj.object_type == CustomObjectTypes.CustomType02
and (not self.loc3Found)):
self.loc3Pose = obj.pose
self.loc3Found = True
self.targetLocation3 = obj
self._tfb.send_transform((x, y, z), q, now, 'Endpoint 3',
self._odom_frame)
print('comzmo has found loc3')
elif (obj.object_type == CustomObjectTypes.CustomType04
and (not self.topBackWall)):
self.topBackPose = obj.pose
self.topBackWallFound = True
self.topBackWall = obj
self._tfb.send_transform((x, y, z), q, now, 'topBackWall',
self._odom_frame)
print('comzmo has found topBackWall')
elif (obj.object_type == CustomObjectTypes.CustomType05
and (not self.frontOfBoxFound)):
self.frontOfBoxPose = obj.pose
self.frontOfBoxFound = True
self.frontOfBox = obj
offset = cozmo.util.Pose(-190, -130, 0, 0, 0, 0, 0)
self.observationPose = self.frontOfBox.pose.define_pose_relative_this(
offset)
self._tfb.send_transform((x, y, z), q, now, 'frontOfBox',
self._odom_frame)
print('comzmo has found frontOfBox')
elif (obj.object_type == CustomObjectTypes.CustomType06
and (not self.topSideWallFound
or not self.topSideWall.pose.is_accurate)):
self.topSideWallPose = obj.pose
self.topSideWallFound = True
self.topSideWall = obj
self._tfb.send_transform((x, y, z), q, now, 'topSideWallk',
self._odom_frame)
print('comzmo has found topSideWalk')
if (self.loc1Found and self.loc2Found and self.loc3Found
and self.frontOfBoxFound):
self.allLocFound = True
# if(self.frontOfBoxFound)
# self.go_to_pose
# else:
# self._cozmo.turn_in_place(degrees(20),in_parallel=True)
def _publish_image(self):
"""
Publish latest camera image as Image with CameraInfo.
#
"""
# only publish if we have a subscriber
if self._image_pub.get_num_connections() == 0:
return
# get latest image from cozmo's camera
camera_image = self._cozmo.world.latest_image
if camera_image is not None:
# convert image to gray scale as it is gray although
img = camera_image.raw_image.convert('L')
ros_img = Image()
ros_img.encoding = 'mono8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = ros_img.width
ros_img.data = img.tobytes()
ros_img.header.frame_id = 'cozmo_camera'
cozmo_time = camera_image.image_recv_time
ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
# publish images and camera info
self._image_pub.publish(ros_img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = ros_img.header
self._camera_info_pub.publish(camera_info)
def _publish_joint_state(self):
"""
Publish joint states as JointStates.
"""
# only publish if we have a subscriber
if self._joint_state_pub.get_num_connections() == 0:
return
js = JointState()
js.header.stamp = rospy.Time.now()
js.header.frame_id = 'cozmo'
js.name = ['head', 'lift']
js.position = [
self._cozmo.head_angle.radians,
self._cozmo.lift_height.distance_mm * 0.001
]
js.velocity = [0.0, 0.0]
js.effort = [0.0, 0.0]
self._joint_state_pub.publish(js)
def _publish_imu(self):
"""
Publish inertia data as Imu message.
"""
# only publish if we have a subscriber
if self._imu_pub.get_num_connections() == 0:
return
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self._base_frame
imu.orientation.w = self._cozmo.pose.rotation.q0
imu.orientation.x = self._cozmo.pose.rotation.q1
imu.orientation.y = self._cozmo.pose.rotation.q2
imu.orientation.z = self._cozmo.pose.rotation.q3
imu.angular_velocity.x = self._cozmo.gyro.x
imu.angular_velocity.y = self._cozmo.gyro.y
imu.angular_velocity.z = self._cozmo.gyro.z
imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
self._imu_pub.publish(imu)
def _publish_battery(self):
"""
Publish battery as BatteryState message.
"""
# only publish if we have a subscriber
if self._battery_pub.get_num_connections() == 0:
return
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
def _publish_odometry(self):
"""
Publish current pose as Odometry message.
"""
# only publish if we have a subscriber
if self._odom_pub.get_num_connections() == 0:
return
now = rospy.Time.now()
odom = Odometry()
odom.header.frame_id = self._odom_frame
odom.header.stamp = now
odom.child_frame_id = self._footprint_frame
odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
1e-1]).ravel()
odom.twist.twist.linear.x = self._lin_vel
odom.twist.twist.angular.z = self._ang_vel
odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
1e-2]).ravel()
self._odom_pub.publish(odom)
def _publish_tf(self, update_rate):
"""
Broadcast current transformations and update
measured velocities for odometry twist.
Published transforms:
odom_frame -> footprint_frame
footprint_frame -> base_frame
base_frame -> head_frame
head_frame -> camera_frame
camera_frame -> camera_optical_frame
"""
now = rospy.Time.now()
x = self._cozmo.pose.position.x * 0.001
y = self._cozmo.pose.position.y * 0.001
z = self._cozmo.pose.position.z * 0.001
# compute current linear and angular velocity from pose change
# Note: Sign for linear velocity is taken from commanded velocities!
# Note: The angular velocity can also be taken from gyroscopes!
delta_pose = self._last_pose - self._cozmo.pose
dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
delta_pose.position.z**2) / 1000.0
self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate
# publish odom_frame -> footprint_frame
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
self._odom_frame)
# publish footprint_frame -> base_frame
q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
self._footprint_frame)
# publish base_frame -> head_frame
q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
self._base_frame)
# publish head_frame -> camera_frame
self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
now, self._camera_frame, self._head_frame)
# publish camera_frame -> camera_optical_frame
q = self._optical_frame_orientation
self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
self._camera_optical_frame,
self._camera_frame)
# store last pose
self._last_pose = deepcopy(self._cozmo.pose)
def _checkDistToLoc(self, num, obj):
if (num == 1):
p1 = np.array([
obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
])
p2 = np.array([
self.loc1Pose.position.x, self.loc1Pose.position.y,
self.loc1Pose.position.z
])
#print(self.loc1Pose.position)
#print(obj.pose.position)
squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
dist = np.sqrt(squared_dis)
#print(dist)
elif (num == 2):
p1 = np.array([
obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
])
p2 = np.array([
self.loc2Pose.position.x, self.loc2Pose.position.y,
self.loc2Pose.position.z
])
squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
dist = np.sqrt(squared_dis)
#print(dist)
elif (num == 3):
p1 = np.array([
obj.pose.position.x, obj.pose.position.y, obj.pose.position.z
])
p2 = np.array([
self.loc3Pose.position.x, self.loc3Pose.position.y,
self.loc3Pose.position.z
])
squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
dist = np.sqrt(squared_dis)
#print(dist)
if (dist < 100):
return False
else:
return True
def _checkDistToSelf(self, obj):
p1 = np.array(
[obj.pose.position.x, obj.pose.position.y, obj.pose.position.z])
p2 = np.array([
self._cozmo.pose.position.x, self._cozmo.pose.position.y,
self._cozmo.pose.position.z
])
dif = abs(self.frontOfBox.pose.position.x - obj.pose.position.x)
#print(self.loc1Pose.position)
#print(obj.pose.position)
squared_dis = ((p2[0] - p1[0])**2) + ((p2[1] - p1[1])**2)
dist = np.sqrt(squared_dis)
print(self.frontOfBox.pose.position)
print(obj.pose.position)
print(dist)
print(dif)
if (dist > 235):
return False
return True
def _findlocations(self):
if (self.allLocFound == False):
self.action = self._cozmo.turn_in_place(degrees(20),
in_parallel=True)
self.action.wait_for_completed()
else:
if (not self.readyToPickUp):
print("locs found moving to ready ")
self.action = self._cozmo.go_to_pose(self.observationPose)
self.action.wait_for_completed()
self.readyToPickUp = True
self._resetHead()
def run(self, update_rate=10):
"""
Publish data continuously with given rate.
:type update_rate: int
:param update_rate: The update rate.
"""
r = rospy.Rate(update_rate)
self._resetHead()
while not rospy.is_shutdown():
self._publish_tf(update_rate)
self._findlocations()
self._publish_image()
self._publish_objects()
self._publish_joint_state()
self._publish_imu()
self._publish_battery()
self._publish_odometry()
self._publish_diagnostics()
# if(self.allLocFound==False):
# self._cozmo.turn_in_place(degrees(20))
# send message repeatedly to avoid idle mode.
# This might cause low battery soon
# TODO improve this! if(not self.allLocFound):
self._cozmo.drive_wheels(*self._wheel_vel)
# sleep
r.sleep()
# stop events on cozmo
self._cozmo.stop_all_motors()
class Camera:
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 get_camera_id(self):
return self._camera_id
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 set_camera_settings(self):
with self._c0:
self._c0.GVSPAdjustPacketSize.run()
while not self._c0.GVSPAdjustPacketSize.is_done():
pass
rospy.loginfo(f"{self._c0.get_pixel_formats()}")
self._c0.StreamBytesPerSecond.set(124000000)
self._c0.set_pixel_format(PixelFormat.BayerRG8) # noqa
self._c0.AcquisitionMode.set("Continuous")
self._c0.ExposureAuto.set("Continuous")
self._c0.Width.set(1210)
self._c0.Height.set(760)
def capture(self, killswitch):
if not self.get_camera():
return
self.set_camera_settings()
with self._c0:
self._c0.start_streaming(handler=self.publish_image)
killswitch.wait()
try:
self._c0.stop_streaming()
except Exception:
pass
def publish_image(self, cam, frame):
rospy.loginfo(
f"Received frame from camera {cam.get_id()}. Publishing to ROS")
img_message = self._bridge.cv2_to_imgmsg(frame.as_numpy_ndarray(),
"bayer_rggb8")
img_message.header.stamp = rospy.Time.now()
self._img_pub.publish(img_message)
self._info_pub.publish(self._info_manager.getCameraInfo())
cam.queue_frame(frame)
class CozmoRos(object):
"""
The Cozmo ROS driver object.
"""
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()
def _publish_diagnostics(self):
# alias
diag_status = self._diag_array.status[0]
# fill diagnostics array
battery_voltage = self._cozmo.battery_voltage
diag_status.values[0].value = '{:.2f} V'.format(battery_voltage)
diag_status.values[1].value = '{:.2f} deg'.format(
self._cozmo.head_angle.degrees)
diag_status.values[2].value = '{:.2f} mm'.format(
self._cozmo.lift_height.distance_mm)
if battery_voltage > 3.5:
diag_status.level = DiagnosticStatus.OK
diag_status.message = 'Everything OK!'
elif battery_voltage > 3.4:
diag_status.level = DiagnosticStatus.WARN
diag_status.message = 'Battery low! Go charge soon!'
else:
diag_status.level = DiagnosticStatus.ERROR
diag_status.message = 'Battery very low! Cozmo will power off soon!'
# update message stamp and publish
self._diag_array.header.stamp = rospy.Time.now()
self._diag_pub.publish(self._diag_array)
def _move_head(self, cmd):
"""
Move head to given angle.
:type cmd: Float64
:param cmd: The message containing angle in degrees. [-25 - 44.5]
"""
action = self._cozmo.set_head_angle(radians(cmd.data * np.pi / 180.),
duration=0.1,
in_parallel=True)
action.wait_for_completed()
def _move_lift(self, cmd):
"""
Move lift to given height.
:type cmd: Float64
:param cmd: A value between [0 - 1], the SDK auto
scales it to the according height.
"""
action = self._cozmo.set_lift_height(height=cmd.data,
duration=0.2,
in_parallel=True)
action.wait_for_completed()
def _set_backpack_led(self, msg):
"""
Set the color of the backpack LEDs.
:type msg: ColorRGBA
:param msg: The color to be set.
"""
# setup color as integer values
color = [int(x * 255) for x in [msg.r, msg.g, msg.b, msg.a]]
# create lights object with duration
light = cozmo.lights.Light(cozmo.lights.Color(rgba=color),
on_period_ms=1000)
# set lights
self._cozmo.set_all_backpack_lights(light)
def _twist_callback(self, cmd):
"""
Set commanded velocities from Twist message.
The commands are actually send/set during run loop, so delay
is in worst case up to 1 / update_rate seconds.
:type cmd: Twist
:param cmd: The commanded velocities.
"""
# compute differential wheel speed
axle_length = 0.07 # 7cm
self._cmd_lin_vel = cmd.linear.x
self._cmd_ang_vel = cmd.angular.z
rv = self._cmd_lin_vel + (self._cmd_ang_vel * axle_length * 0.5)
lv = self._cmd_lin_vel - (self._cmd_ang_vel * axle_length * 0.5)
self._wheel_vel = (lv * 1000., rv * 1000.) # convert to mm / s
def _say_callback(self, msg):
"""
The callback for incoming text messages to be said.
:type msg: String
:param msg: The text message to say.
"""
self._cozmo.say_text(msg.data).wait_for_completed()
def _publish_objects(self):
"""
Publish detected object as transforms between odom_frame and object_frame.
"""
for obj in self._cozmo.world.visible_objects:
now = rospy.Time.now()
x = obj.pose.position.x * 0.001
y = obj.pose.position.y * 0.001
z = obj.pose.position.z * 0.001
q = (obj.pose.rotation.q1, obj.pose.rotation.q2,
obj.pose.rotation.q3, obj.pose.rotation.q0)
self._tfb.send_transform((x, y, z), q, now,
'cube_' + str(obj.object_id),
self._odom_frame)
def _publish_image(self):
"""
Publish latest camera image as Image with CameraInfo.
"""
# only publish if we have a subscriber
if self._image_pub.get_num_connections() == 0:
return
# get latest image from cozmo's camera
camera_image = self._cozmo.world.latest_image
if camera_image is not None:
# convert image to gray scale as it is gray although
img = camera_image.raw_image.convert('L')
ros_img = Image()
ros_img.encoding = 'mono8'
ros_img.width = img.size[0]
ros_img.height = img.size[1]
ros_img.step = ros_img.width
ros_img.data = img.tobytes()
ros_img.header.frame_id = 'cozmo_camera'
cozmo_time = camera_image.image_recv_time
ros_img.header.stamp = rospy.Time.from_sec(cozmo_time)
# publish images and camera info
self._image_pub.publish(ros_img)
camera_info = self._camera_info_manager.getCameraInfo()
camera_info.header = ros_img.header
self._camera_info_pub.publish(camera_info)
def _publish_joint_state(self):
"""
Publish joint states as JointStates.
"""
# only publish if we have a subscriber
if self._joint_state_pub.get_num_connections() == 0:
return
js = JointState()
js.header.stamp = rospy.Time.now()
js.header.frame_id = 'cozmo'
js.name = ['head', 'lift']
js.position = [
self._cozmo.head_angle.radians,
self._cozmo.lift_height.distance_mm * 0.001
]
js.velocity = [0.0, 0.0]
js.effort = [0.0, 0.0]
self._joint_state_pub.publish(js)
def _publish_imu(self):
"""
Publish inertia data as Imu message.
"""
# only publish if we have a subscriber
if self._imu_pub.get_num_connections() == 0:
return
imu = Imu()
imu.header.stamp = rospy.Time.now()
imu.header.frame_id = self._base_frame
imu.orientation.w = self._cozmo.pose.rotation.q0
imu.orientation.x = self._cozmo.pose.rotation.q1
imu.orientation.y = self._cozmo.pose.rotation.q2
imu.orientation.z = self._cozmo.pose.rotation.q3
imu.angular_velocity.x = self._cozmo.gyro.x
imu.angular_velocity.y = self._cozmo.gyro.y
imu.angular_velocity.z = self._cozmo.gyro.z
imu.linear_acceleration.x = self._cozmo.accelerometer.x * 0.001
imu.linear_acceleration.y = self._cozmo.accelerometer.y * 0.001
imu.linear_acceleration.z = self._cozmo.accelerometer.z * 0.001
self._imu_pub.publish(imu)
def _publish_battery(self):
"""
Publish battery as BatteryState message.
"""
# only publish if we have a subscriber
if self._battery_pub.get_num_connections() == 0:
return
battery = BatteryState()
battery.header.stamp = rospy.Time.now()
battery.voltage = self._cozmo.battery_voltage
battery.present = True
if self._cozmo.is_on_charger: # is_charging always return False
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_CHARGING
else:
battery.power_supply_status = BatteryState.POWER_SUPPLY_STATUS_NOT_CHARGING
self._battery_pub.publish(battery)
def _publish_odometry(self):
"""
Publish current pose as Odometry message.
"""
# only publish if we have a subscriber
if self._odom_pub.get_num_connections() == 0:
return
now = rospy.Time.now()
odom = Odometry()
odom.header.frame_id = self._odom_frame
odom.header.stamp = now
odom.child_frame_id = self._footprint_frame
odom.pose.pose.position.x = self._cozmo.pose.position.x * 0.001
odom.pose.pose.position.y = self._cozmo.pose.position.y * 0.001
odom.pose.pose.position.z = self._cozmo.pose.position.z * 0.001
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.pose.covariance = np.diag([1e-2, 1e-2, 1e-2, 1e3, 1e3,
1e-1]).ravel()
odom.twist.twist.linear.x = self._lin_vel
odom.twist.twist.angular.z = self._ang_vel
odom.twist.covariance = np.diag([1e-2, 1e3, 1e3, 1e3, 1e3,
1e-2]).ravel()
self._odom_pub.publish(odom)
def _publish_tf(self, update_rate):
"""
Broadcast current transformations and update
measured velocities for odometry twist.
Published transforms:
odom_frame -> footprint_frame
footprint_frame -> base_frame
base_frame -> head_frame
head_frame -> camera_frame
camera_frame -> camera_optical_frame
"""
now = rospy.Time.now()
x = self._cozmo.pose.position.x * 0.001
y = self._cozmo.pose.position.y * 0.001
z = self._cozmo.pose.position.z * 0.001
# compute current linear and angular velocity from pose change
# Note: Sign for linear velocity is taken from commanded velocities!
# Note: The angular velocity can also be taken from gyroscopes!
delta_pose = self._last_pose - self._cozmo.pose
dist = np.sqrt(delta_pose.position.x**2 + delta_pose.position.y**2 +
delta_pose.position.z**2) / 1000.0
self._lin_vel = dist * update_rate * np.sign(self._cmd_lin_vel)
self._ang_vel = -delta_pose.rotation.angle_z.radians * update_rate
# publish odom_frame -> footprint_frame
q = quaternion_from_euler(.0, .0, self._cozmo.pose_angle.radians)
self._tfb.send_transform((x, y, 0.0), q, now, self._footprint_frame,
self._odom_frame)
# publish footprint_frame -> base_frame
q = quaternion_from_euler(.0, -self._cozmo.pose_pitch.radians, .0)
self._tfb.send_transform((0.0, 0.0, 0.02), q, now, self._base_frame,
self._footprint_frame)
# publish base_frame -> head_frame
q = quaternion_from_euler(.0, -self._cozmo.head_angle.radians, .0)
self._tfb.send_transform((0.02, 0.0, 0.05), q, now, self._head_frame,
self._base_frame)
# publish head_frame -> camera_frame
self._tfb.send_transform((0.025, 0.0, -0.015), (0.0, 0.0, 0.0, 1.0),
now, self._camera_frame, self._head_frame)
# publish camera_frame -> camera_optical_frame
q = self._optical_frame_orientation
self._tfb.send_transform((0.0, 0.0, 0.0), q, now,
self._camera_optical_frame,
self._camera_frame)
# store last pose
self._last_pose = deepcopy(self._cozmo.pose)
def run(self, update_rate=10):
"""
Publish data continuously with given rate.
:type update_rate: int
:param update_rate: The update rate.
"""
r = rospy.Rate(update_rate)
while not rospy.is_shutdown():
self._publish_tf(update_rate)
self._publish_image()
self._publish_objects()
self._publish_joint_state()
self._publish_imu()
self._publish_battery()
self._publish_odometry()
self._publish_diagnostics()
# send message repeatedly to avoid idle mode.
# This might cause low battery soon
# TODO improve this!
self._cozmo.drive_wheels(*self._wheel_vel)
# sleep
r.sleep()
# stop events on cozmo
self._cozmo.stop_all_motors()
class UVCCamNode:
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
#self.rate = rospy.Rate(60)
def callback(self, config, level):
rospy.loginfo("""Reconfigure Request: \
{exposure}, \
{auto_exposure}, \
{brightness}, \
{backlight_comp}, \
{gamma}, \
{contrast}, \
{sharpness}, \
{hue} \
{saturation} \
{pwr_line_freq} \
{auto_exp_prio} \
{white_bal_auto} \
{white_bal} \
""".format(**config))
self.controls_dict['Absolute Exposure Time'].value = config.exposure
self.controls_dict['Auto Exposure Mode'].value = config.auto_exposure
self.controls_dict['Brightness'].value = config.brightness
self.controls_dict[
'Backlight Compensation'].value = config.backlight_comp
self.controls_dict['Gamma'].value = config.gamma
self.controls_dict['Contrast'].value = config.contrast
self.controls_dict['Sharpness'].value = config.sharpness
self.controls_dict['Hue'].value = config.hue
self.controls_dict['Saturation'].value = config.saturation
self.controls_dict['Power Line frequency'].value = config.pwr_line_freq
self.controls_dict[
'Auto Exposure Priority'].value = config.auto_exp_prio
self.controls_dict[
'White Balance temperature,Auto'].value = config.white_bal_auto
self.controls_dict[
'White Balance temperature'].value = config.white_bal
for c in self.cap.controls:
rospy.loginfo('%s: %i' % (c.display_name, c.value))
return config
def read_and_publish_image(self):
# Read image from camera
self.frame = self.cap.get_frame_robust()
if (self.counter % 2) == 0:
capture_time = rospy.Time.now()
# Convert numpy image to ROS image message
self.image_msg = self.bridge.cv2_to_imgmsg(self.frame.gray,
encoding=self.encoding)
# Add timestamp and sequence number (empty by default)
self.image_msg.header.stamp = capture_time
self.image_msg.header.seq = self.seq
self.image_publisher.publish(self.image_msg)
camera_msg = self.camera_info
camera_msg.header = self.image_msg.header # Copy header from image message
self.camera_publisher.publish(camera_msg)
if self.seq == 0:
rospy.loginfo(
"Publishing images from UVC Cam at '/{}/{}' ".format(
DEFAULT_CAMERA_NAME, self.image_topic))
self.seq += 1
self.counter += 1
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
# if k == 27:
# break
cv2.destroyAllWindows()
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())