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)
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()
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)
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 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
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()
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...")