How to use

getSensorInformation

method

in

us.ihmc.avatar.drcRobot.DRCRobotModel

public WandererSensorReaderFactory(DRCRobotModel robotModel, CostOfTransportCalculator costOfTransportCalculator)
{
 sensorInformation = robotModel.getSensorInformation();
 stateEstimatorParameters = robotModel.getStateEstimatorParameters();
 this.costOfTransportCalculator = costOfTransportCalculator;
}

public StepprSensorReaderFactory(DRCRobotModel robotModel)
{
 sensorInformation = robotModel.getSensorInformation();
 stateEstimatorParameters = robotModel.getStateEstimatorParameters();
}

protected void setupSubscribers(ConnectedNode connectedNode)
{
 DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation();
 DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null)
 {
   jointSubscriber = connectedNode.newSubscriber(lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE);
 }
}

protected void setupSubscribers(ConnectedNode connectedNode)
{
 DRCRobotSensorInformation sensorInforamtion = robotModel.getSensorInformation();
 DRCRobotLidarParameters lidarParameters = sensorInforamtion.getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointTopic() != null)
 {
   jointSubscriber = connectedNode.newSubscriber(lidarParameters.getLidarSpindleJointTopic(), sensor_msgs.JointState._TYPE);
 }
}

private SideDependentList<WristForceSensorFilteredUpdatable> createWristForceSensorUpdateables()
{
 SideDependentList<WristForceSensorFilteredUpdatable> ret = new SideDependentList<WristForceSensorFilteredUpdatable>();
 for (RobotSide robotSide : RobotSide.values)
 {
   WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcRobotModel.getSimpleRobotName(), robotSide,
                                                  fullRobotModel, drcRobotModel.getSensorInformation(),
                                                  robotDataReceiver.getForceSensorDataHolder(),
                                                  IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT,
                                                  ros2Node, registry);
   ret.put(robotSide, wristSensorUpdatable);
 }
 return ret;
}

private void setupControllerThread()
{
 controllerThread = new DRCControllerThread(robotModel.get(), robotModel.get().getSensorInformation(), momentumBasedControllerFactory.get(),
                       threadDataSynchronizer, simulationOutputWriter, humanoidGlobalDataProducer.get(), yoVariableServer,
                       gravity.get(), robotModel.get().getEstimatorDT());
}

                                           robotModel.getEstimatorDT());
DRCEstimatorThread estimatorThread = new DRCEstimatorThread(robotModel.getSimpleRobotName(), robotModel.getSensorInformation(),
                              robotModel.getContactPointParameters(), robotModel, robotModel.getStateEstimatorParameters(),
                              sensorReaderFactory, threadDataSynchronizer, realtimeRos2Node, null, null, robotVisualizer,

private void setupLidarController()
{
 DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null)
 {
   PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot,
                                           lidarParameters.getLidarSpindleJointName(),
                                           lidarParameters.getLidarSpindleVelocity(),
                                           robotModel.get().getSimulateDT());
   humanoidFloatingRootJointRobot.setController(pidLidarTorqueController);
 }
}

private void setupLidarController()
{
 DRCRobotLidarParameters lidarParameters = robotModel.get().getSensorInformation().getLidarParameters(0);
 if (lidarParameters != null && lidarParameters.getLidarSpindleJointName() != null)
 {
   PIDLidarTorqueController pidLidarTorqueController = new PIDLidarTorqueController(humanoidFloatingRootJointRobot,
                                           lidarParameters.getLidarSpindleJointName(),
                                           lidarParameters.getLidarSpindleVelocity(),
                                           robotModel.get().getSimulateDT());
   humanoidFloatingRootJointRobot.setController(pidLidarTorqueController);
 }
}

private void setupControllerThread()
{
 String robotName = robotModel.get().getSimpleRobotName();
 controllerThread = new DRCControllerThread(robotName, robotModel.get(), robotModel.get().getSensorInformation(), highLevelHumanoidControllerFactory.get(),
                       threadDataSynchronizer, simulationOutputProcessor, realtimeRos2Node.get(), yoVariableServer, gravity.get(),
                       robotModel.get().getEstimatorDT());
}

@Before
public void setUp()
{
 registry = new YoVariableRegistry(getClass().getSimpleName());
 this.yoTime = new YoDouble("yoTime", registry);
 this.ros2Node = ROS2Tools.createRos2Node(PubSubImplementation.INTRAPROCESS, "ihmc_humanoid_behavior_dispatcher_test");
 drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel());
 drcSimulationTestHelper.createSimulation(getSimpleRobotName());
 MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(getClass().getSimpleName() + " before test.");
 robot = drcSimulationTestHelper.getRobot();
 fullRobotModel = getRobotModel().createFullRobotModel();
 walkingControllerParameters = getRobotModel().getWalkingControllerParameters();
 yoGraphicsListRegistry = new YoGraphicsListRegistry();
 behaviorDispatcher = setupBehaviorDispatcher(getRobotModel().getSimpleRobotName(), fullRobotModel, ros2Node, yoGraphicsListRegistry, registry);
 CapturePointUpdatable capturePointUpdatable = createCapturePointUpdateable(drcSimulationTestHelper, registry, yoGraphicsListRegistry);
 behaviorDispatcher.addUpdatable(capturePointUpdatable);
 DRCRobotSensorInformation sensorInfo = getRobotModel().getSensorInformation();
 for (RobotSide robotSide : RobotSide.values)
 {
   WristForceSensorFilteredUpdatable wristSensorUpdatable = new WristForceSensorFilteredUpdatable(drcSimulationTestHelper.getRobotName(), robotSide,
                                                  fullRobotModel, sensorInfo,
                                                  robotDataReceiver.getForceSensorDataHolder(),
                                                  IHMCHumanoidBehaviorManager.BEHAVIOR_YO_VARIABLE_SERVER_DT,
                                                  drcSimulationTestHelper.getRos2Node(), registry);
   behaviorDispatcher.addUpdatable(wristSensorUpdatable);
 }
 referenceFrames = robotDataReceiver.getReferenceFrames();
}

private void setupBehaviorModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException
{
 if (params.isBehaviorModuleEnabled())
 {
   DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
   LogModelProvider logModelProvider = robotModel.getLogModelProvider();
   if (params.isAutomaticDiagnosticEnabled())
   {
    IHMCHumanoidBehaviorManager.createBehaviorModuleForAutomaticDiagnostic(robotModel.getSimpleRobotName(), robotModel, robotModel, logModelProvider,
                                        params.isBehaviorVisualizerEnabled(), sensorInformation,
                                        params.getTimeToWaitBeforeStartingDiagnostics());
   }
   else
   {
    new IHMCHumanoidBehaviorManager(robotModel.getSimpleRobotName(), robotModel, robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(),
                    sensorInformation);
   }
   String methodName = "setupBehaviorModule ";
   printModuleConnectedDebugStatement(PacketDestination.BEHAVIOR_MODULE, methodName);
 }
}

HighLevelControllerParameters highLevelControllerParameters = robotModel.getHighLevelControllerParameters();
DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
SideDependentList<String> feetForceSensorNames = sensorInformation.getFeetForceSensorNames();
SideDependentList<String> feetContactSensorNames = sensorInformation.getFeetContactSensorNames();

private void createControllerCore(YoVariableRegistry walkingControllerRegistry)
{
 JointBasics[] jointsToIgnore = DRCControllerThread.createListOfJointsToIgnore(fullRobotModel, robotModel, robotModel.getSensorInformation());
 JointBasics[] jointsToOptimizeFor = HighLevelHumanoidControllerToolbox.computeJointsToOptimizeFor(fullRobotModel, jointsToIgnore);
 FloatingJointBasics rootJoint = fullRobotModel.getRootJoint();
 ReferenceFrame centerOfMassFrame = referenceFrames.getCenterOfMassFrame();
 WalkingControllerParameters walkingControllerParameters = robotModel.getWalkingControllerParameters();
 MomentumOptimizationSettings momentumOptimizationSettings = walkingControllerParameters.getMomentumOptimizationSettings();
 WholeBodyControlCoreToolbox toolbox = new WholeBodyControlCoreToolbox(controlDT, gravityZ, rootJoint, jointsToOptimizeFor, centerOfMassFrame,
                                    momentumOptimizationSettings, yoGraphicsListRegistry, walkingControllerRegistry);
 toolbox.setupForInverseDynamicsSolver(contactableBodies);
 JointPrivilegedConfigurationParameters jointPrivilegedConfigurationParameters = walkingControllerParameters.getJointPrivilegedConfigurationParameters();
 toolbox.setJointPrivilegedConfigurationParameters(jointPrivilegedConfigurationParameters);
 FeedbackControlCommandList template = managerFactory.createFeedbackControlTemplate();
 controllerOutput = new JointDesiredOutputList(fullRobotModel.getControllableOneDoFJoints());
 controllerCore = new WholeBodyControllerCore(toolbox, template, controllerOutput, walkingControllerRegistry);
}

private void setupBehaviorModule(DRCRobotModel robotModel, DRCNetworkModuleParameters params) throws IOException
{
 if (params.isBehaviorModuleEnabled())
 {
   DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
   LogModelProvider logModelProvider = robotModel.getLogModelProvider();
   if (params.isAutomaticDiagnosticEnabled())
   {
    IHMCHumanoidBehaviorManager
       .createBehaviorModuleForAutomaticDiagnostic(robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation,
                             params.getTimeToWaitBeforeStartingDiagnostics());
   }
   else
   {
    new IHMCHumanoidBehaviorManager(robotModel, logModelProvider, params.isBehaviorVisualizerEnabled(), sensorInformation);
   }
   PacketCommunicator behaviorModuleCommunicator = PacketCommunicator
      .createIntraprocessPacketCommunicator(NetworkPorts.BEHAVIOUR_MODULE_PORT, NET_CLASS_LIST);
   packetRouter.attachPacketCommunicator(PacketDestination.BEHAVIOR_MODULE, behaviorModuleCommunicator);
   behaviorModuleCommunicator.connect();
   String methodName = "setupBehaviorModule ";
   printModuleConnectedDebugStatement(PacketDestination.BEHAVIOR_MODULE, methodName);
 }
}

private void setupStateEstimationThread()
{
 stateEstimationThread = new DRCEstimatorThread(robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(),
                         robotModel.get().getStateEstimatorParameters(), sensorReaderFactory, threadDataSynchronizer,
                         new PeriodicNonRealtimeThreadScheduler("DRCSimGazeboYoVariableServer"), humanoidGlobalDataProducer.get(),
                         yoVariableServer, gravity.get());
 if (humanoidGlobalDataProducer.get() != null)
 {
   PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(humanoidGlobalDataProducer.get());
   humanoidGlobalDataProducer.get().attachListener(StampedPosePacket.class, pelvisPoseCorrectionCommunicator);
   stateEstimationThread.setExternalPelvisCorrectorSubscriber(pelvisPoseCorrectionCommunicator);
 }
 else
 {
   stateEstimationThread.setExternalPelvisCorrectorSubscriber(null);
 }
}

public LocalObjectCommunicator createSimulatedSensorsPacketCommunicator()
{
 scsSensorOutputPacketCommunicator = new LocalObjectCommunicator();
 if (createSCSSimulatedSensors)
 {
   DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
   DRCRobotJointMap jointMap = robotModel.getJointMap();
   TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider();
   HumanoidFloatingRootJointRobot robot = avatarSimulation.getHumanoidFloatingRootJointRobot();
   Graphics3DAdapter graphics3dAdapter = simulationConstructionSet.getGraphics3dAdapter();
   printIfDebug("Streaming SCS Video");
   DRCRobotCameraParameters cameraParameters = sensorInformation.getCameraParameters(0);
   if (cameraParameters != null)
   {
    String cameraName = cameraParameters.getSensorNameInSdf();
    int width = sdfRobot.getCameraMount(cameraName).getImageWidth();
    int height = sdfRobot.getCameraMount(cameraName).getImageHeight();
    CameraConfiguration cameraConfiguration = new CameraConfiguration(cameraName);
    cameraConfiguration.setCameraMount(cameraName);
    int framesPerSecond = 25;
    DRCRenderedSceneVideoHandler drcRenderedSceneVideoHandler = new DRCRenderedSceneVideoHandler(scsSensorOutputPacketCommunicator);
    simulationConstructionSet.startStreamingVideoData(cameraConfiguration, width, height, drcRenderedSceneVideoHandler, timeStampProvider, framesPerSecond);
   }
   for (DRCRobotLidarParameters lidarParams : sensorInformation.getLidarParameters())
   {
    DRCLidar.setupDRCRobotLidar(robot, graphics3dAdapter, scsSensorOutputPacketCommunicator, jointMap, lidarParams, timeStampProvider, true);
   }
 }
 return scsSensorOutputPacketCommunicator;
}

DRCRobotSensorInformation robotSensorInfo = robotModel.getSensorInformation();
FlatGroundEnvironment flatGround = new FlatGroundEnvironment();
DRCNetworkModuleParameters networkModuleParameters = new DRCNetworkModuleParameters();

private void setupStateEstimationThread()
{
 String robotName = robotModel.get().getSimpleRobotName();
 MessageTopicNameGenerator publisherTopicNameGenerator = ControllerAPIDefinition.getPublisherTopicNameGenerator(robotName);
 MessageTopicNameGenerator subscriberTopicNameGenerator = ControllerAPIDefinition.getSubscriberTopicNameGenerator(robotName);
 PelvisPoseCorrectionCommunicatorInterface pelvisPoseCorrectionCommunicator;
 if (externalPelvisCorrectorSubscriber.hasValue())
 {
   pelvisPoseCorrectionCommunicator = externalPelvisCorrectorSubscriber.get();
 }
 else
 {
   pelvisPoseCorrectionCommunicator = new PelvisPoseCorrectionCommunicator(realtimeRos2Node.get(), publisherTopicNameGenerator);
   ROS2Tools.createCallbackSubscription(realtimeRos2Node.get(), StampedPosePacket.class, subscriberTopicNameGenerator,
                     s -> pelvisPoseCorrectionCommunicator.receivedPacket(s.takeNextData()));
 }
 stateEstimationThread = new DRCEstimatorThread(robotName, robotModel.get().getSensorInformation(), robotModel.get().getContactPointParameters(),
                         robotModel.get(), robotModel.get().getStateEstimatorParameters(), sensorReaderFactory,
                         threadDataSynchronizer, realtimeRos2Node.get(), pelvisPoseCorrectionCommunicator, simulationOutputWriter,
                         yoVariableServer, gravity.get());
}

DRCRobotSensorInformation sensorInformation = robotModel.getSensorInformation();
DRCRobotJointMap jointMap = robotModel.getJointMap();
TimestampProvider timeStampProvider = avatarSimulation.getSimulatedRobotTimeProvider();