public GetLidarScanExampleBehavior(String robotName, Ros2Node ros2Node) { super(robotName, ros2Node); // coactiveBehaviorsNetworkManager = ros2Node; FIXME createSubscriber(PointCloudWorldPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), pointCloudQueue::put); }
public ObjectWeightBehavior(String robotName, Ros2Node ros2Node) { super(robotName, ros2Node); publisher = createPublisher(ObjectWeightPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
public GetVideoPacketExampleBehavior(String robotName, Ros2Node ros2Node) { super(robotName, ros2Node); // coactiveBehaviorsNetworkManager = ros2Node; FIXME createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), videoPacketQueue::put); }
public CompressedFisheyeHandler(Ros2Node ros2Node) { publisher = ROS2Tools.createPublisher(ros2Node, FisheyePacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
public VideoPacketHandler(Ros2Node ros2Node) { publisher = ROS2Tools.createPublisher(ros2Node, VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
public VideoPacketListenerBehavior(String robotName, String namePrefix, Ros2Node ros2Node) { super(robotName, namePrefix, ros2Node); videoDataClient = CompressedVideoDataFactory.createCompressedVideoDataClient(this); createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), cameraData::put); }
public SphereDetectionBehavior(String robotName, Ros2Node ros2Node, HumanoidReferenceFrames referenceFrames) { super(robotName, ros2Node); createSubscriber(PointCloudWorldPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), pointCloudQueue::put); detectedObjectPublisher = createBehaviorOutputPublisher(DetectedObjectPacket.class); this.humanoidReferenceFrames = referenceFrames; }
public CollaborativeBehavior(String robotName, Ros2Node ros2Node, HumanoidReferenceFrames referenceFrames, FullHumanoidRobotModel fullHumanoidRobotModel, DRCRobotSensorInformation robotSensorInfo, WalkingControllerParameters walkingControllerParameters, YoGraphicsListRegistry graphicsListRegistry) { super(robotName, ros2Node); createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), cameraData::put); DRCRobotCameraParameters[] robotCameraParameters = robotSensorInfo.getCameraParameters(); }
public MultisensePointCloud2WithSourceReceiver() throws URISyntaxException, IOException { super(PointCloud2WithSource._TYPE); URI masterURI = new URI("http://10.6.192.14:11311"); RosMainNode rosMainNode = new RosMainNode(masterURI, "LidarScanPublisher", true); rosMainNode.attachSubscriber("/singleScanAsCloudWithSource", this); rosMainNode.execute(); lidarScanPublisher = ROS2Tools.createPublisher(ros2Node, LidarScanMessage.class, ROS2Tools.getDefaultTopicNameGenerator()); }
public TextToSpeechNetworkModule() { ROS2Tools.createCallbackSubscription(ros2Node, TextToSpeechPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), s -> receivedPacket(s.takeNextData())); }
public void startServer(NetworkPorts portToOpen) throws IOException { if (ros2Node != null) { LogTools.error("There is already a log server running."); } else { ros2Node = ROS2Tools.createRealtimeRos2Node(PubSubImplementation.FAST_RTPS, "lidar_log"); lidarScanPublisher = ROS2Tools.createPublisher(ros2Node, LidarScanMessage.class, ROS2Tools.getDefaultTopicNameGenerator()); ros2Node.spin(); } if (currentLoggingTask == null) currentLoggingTask = executorService.scheduleAtFixedRate(this::readData, 0L, 10L, TimeUnit.MILLISECONDS); }
public BlobFilteredSphereDetectionBehavior(String robotName, Ros2Node ros2Node, HumanoidReferenceFrames referenceFrames, FullHumanoidRobotModel fullRobotModel) { super(robotName, ros2Node, referenceFrames); createSubscriber(PointCloudWorldPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), pointCloudQueue::put); // FIXME That stream is no more coloredCircularBlobDetectorBehaviorService = new ColoredCircularBlobDetectorBehaviorService(robotName, ros2Node); this.headFrame = fullRobotModel.getHead().getBodyFixedFrame(); }
public ColoredCircularBlobDetectorBehaviorService(String robotName, Ros2Node ros2Node) { super(robotName, ColoredCircularBlobDetectorBehaviorService.class.getSimpleName(), ros2Node); createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), videoPacketQueue::put); // FIXME Need to figure out the topic name for video streams createSubscriberFromController(RobotConfigurationData.class, robotConfigurationDataQueue::put); videoPublisher = createBehaviorOutputPublisher(VideoPacket.class, "/video"); OpenCVColoredCircularBlobDetectorFactory factory = new OpenCVColoredCircularBlobDetectorFactory(); factory.setCaptureSource(OpenCVColoredCircularBlobDetector.CaptureSource.JAVA_BUFFERED_IMAGES); openCVColoredCircularBlobDetector = factory.buildBlobDetector(); }
private void setupRosLocalization() { new IHMCETHRosLocalizationUpdateSubscriber(robotName, rosMainNode, ros2Node, ppsTimestampOffsetProvider); RosLocalizationServiceClient rosLocalizationServiceClient = new RosLocalizationServiceClient(rosMainNode); ROS2Tools.createCallbackSubscription(ros2Node, LocalizationPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), s -> rosLocalizationServiceClient.receivedPacket(s.takeNextData())); }
public FiducialDetectorBehaviorService(String robotName, Ros2Node ros2Node, YoGraphicsListRegistry yoGraphicsListRegistry) { super(robotName, FiducialDetectorBehaviorService.class.getSimpleName(), ros2Node); createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), videoPacketQueue::put); transformFromReportedToFiducialFrame = new RigidBodyTransform(); fiducialDetectorFromCameraImages = new FiducialDetectorFromCameraImages(transformFromReportedToFiducialFrame, getYoVariableRegistry(), yoGraphicsListRegistry); fiducialDetectorFromCameraImages.setFieldOfView(DEFAULT_FIELD_OF_VIEW_X_IN_RADIANS, DEFAULT_FIELD_OF_VIEW_Y_IN_RADIANS); fiducialDetectorFromCameraImages.setExpectedFiducialSize(DEFAULT_FIDUCIAL_SIZE); String prefix = "fiducial"; locationEnabled = new YoBoolean(prefix + "LocationEnabled", getYoVariableRegistry()); locationEnabled.set(false); }
public StereoVisionPointCloudPublisher(FullHumanoidRobotModelFactory modelFactory, Ros2Node ros2Node, String robotConfigurationDataTopicName) { robotName = modelFactory.getRobotDescription().getName(); fullRobotModel = modelFactory.createFullRobotModel(); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, robotConfigurationDataTopicName, s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); pointcloudPublisher = ROS2Tools.createPublisher(ros2Node, StereoVisionPointCloudMessage.class, ROS2Tools.getDefaultTopicNameGenerator(robotName)); }
public AbstractBehavior(String robotName, String namePrefix, Ros2Node ros2Node) { this.robotName = robotName; this.ros2Node = ros2Node; behaviorName = FormattingTools.addPrefixAndKeepCamelCaseForMiddleOfExpression(namePrefix, getClass().getSimpleName()); registry = new YoVariableRegistry(behaviorName); yoBehaviorStatus = new YoEnum<BehaviorStatus>(namePrefix + "Status", registry, BehaviorStatus.class); hasBeenInitialized = new YoBoolean("hasBeenInitialized", registry); isPaused = new YoBoolean("isPaused" + behaviorName, registry); isAborted = new YoBoolean("isAborted" + behaviorName, registry); percentCompleted = new YoDouble("percentCompleted", registry); behaviorsServices = new ArrayList<>(); footstepPlanningToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2TopicQualifier.INPUT); footstepPlanningToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.FOOTSTEP_PLANNER_TOOLBOX, ROS2TopicQualifier.OUTPUT); kinematicsToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_TOOLBOX, ROS2TopicQualifier.INPUT); kinematicsToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_TOOLBOX, ROS2TopicQualifier.OUTPUT); kinematicsPlanningToolboxSubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_PLANNING_TOOLBOX, ROS2TopicQualifier.INPUT); kinematicsPlanningToolboxPubGenerator = ROS2Tools.getTopicNameGenerator(robotName, ROS2Tools.KINEMATICS_PLANNING_TOOLBOX, ROS2TopicQualifier.OUTPUT); controllerSubGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName); controllerPubGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName); behaviorSubGenerator = IHMCHumanoidBehaviorManager.getSubscriberTopicNameGenerator(robotName); behaviorPubGenerator = IHMCHumanoidBehaviorManager.getPublisherTopicNameGenerator(robotName); textToSpeechPublisher = createPublisher(TextToSpeechPacket.class, ROS2Tools.getDefaultTopicNameGenerator()); }
public ObjectDetectorBehaviorService(String robotName, Ros2Node ros2Node, YoGraphicsListRegistry yoGraphicsListRegistry) throws Exception { super(robotName, ObjectDetectorBehaviorService.class.getSimpleName(), ros2Node); createSubscriber(VideoPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), videoPacketQueue::put); transformFromReportedToFiducialFrame = new RigidBodyTransform(); objectDetectorFromCameraImages = new ObjectDetectorFromCameraImages(transformFromReportedToFiducialFrame, getYoVariableRegistry(), yoGraphicsListRegistry); objectDetectorFromCameraImages.setFieldOfView(DEFAULT_FIELD_OF_VIEW_X_IN_RADIANS, DEFAULT_FIELD_OF_VIEW_Y_IN_RADIANS); objectDetectorFromCameraImages.setExpectedObjectSize(DEFAULT_OBJECT_SIZE); createSubscriber(ObjectDetectorResultPacket.class, ROS2Tools.getDefaultTopicNameGenerator(), objectDetectorFromCameraImages); String prefix = "fiducial"; locationEnabled = new YoBoolean(prefix + "LocationEnabled", getYoVariableRegistry()); shouldRecordVideoPackets = new YoBoolean("ShouldRecordVideoPackets", getYoVariableRegistry()); shouldRecordVideoPackets.set(false); locationEnabled.set(false); IHMCROS2Publisher<BoundingBoxesPacket> boundingBoxesPublisher = createBehaviorOutputPublisher(BoundingBoxesPacket.class, "/bounding_boxes"); IHMCROS2Publisher<HeatMapPacket> heatMapPublisher = createBehaviorOutputPublisher(HeatMapPacket.class, "/heat_map"); objectDetectorFromCameraImages.addDetectionResultListener(detectionVisualizationPackets -> { boundingBoxesPublisher.publish(detectionVisualizationPackets.getBoundingBoxesPacket()); heatMapPublisher.publish(detectionVisualizationPackets.getHeatMapPacket()); }); }
public LidarScanPublisher(String lidarName, FullHumanoidRobotModelFactory modelFactory, Ros2Node ros2Node, String robotConfigurationDataTopicName) { robotName = modelFactory.getRobotDescription().getName(); PrintTools.info(robotName); fullRobotModel = modelFactory.createFullRobotModel(); lidarBaseFrame = fullRobotModel.getLidarBaseFrame(lidarName); RigidBodyTransform transformToLidarBaseFrame = fullRobotModel.getLidarBaseToSensorTransform(lidarName); lidarSensorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("lidarSensorFrame", lidarBaseFrame, transformToLidarBaseFrame); ROS2Tools.createCallbackSubscription(ros2Node, RobotConfigurationData.class, robotConfigurationDataTopicName, s -> robotConfigurationDataBuffer.receivedPacket(s.takeNextData())); lidarScanPublisher = ROS2Tools.createPublisher(ros2Node, LidarScanMessage.class, ROS2Tools.getDefaultTopicNameGenerator()); }
yoGraphicsListRegistry.registerYoGraphic("Regions", yoGraphicPlanarRegionsList); lidarScanPublisher = ROS2Tools.createPublisher(ros2Node, LidarScanMessage.class, ROS2Tools.getDefaultTopicNameGenerator());