From 08a0dfc6a89fe121a71da19497e3f421380c0295 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 17 Feb 2022 09:30:29 +0100 Subject: [PATCH 1/2] JointStatePublisher publish parent, child and axis data (#1345) Signed-off-by: ahcorde --- .../JointStatePublisher.cc | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 9830edfbd7..ce0eb759a0 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -24,12 +24,15 @@ #include +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointForce.hh" #include "ignition/gazebo/components/JointPosition.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Pose.hh" using namespace ignition; @@ -184,6 +187,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, if (pose) msgs::Set(jointMsg->mutable_pose(), pose->Data()); + auto child = _ecm.Component(joint); + if (child) + { + jointMsg->set_child(child->Data()); + } + + auto parent = _ecm.Component(joint); + if (parent) + { + jointMsg->set_parent(parent->Data()); + } + // Set the joint position const auto *jointPositions = _ecm.Component(joint); @@ -194,6 +209,19 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, if (i == 0) { jointMsg->mutable_axis1()->set_position(jointPositions->Data()[i]); + auto jointAxis = _ecm.Component(joint); + if (jointAxis) + { + msgs::Set( + jointMsg->mutable_axis1()->mutable_xyz(), + jointAxis->Data().Xyz()); + jointMsg->mutable_axis1()->set_limit_upper( + jointAxis->Data().Upper()); + jointMsg->mutable_axis1()->set_limit_lower( + jointAxis->Data().Lower()); + jointMsg->mutable_axis1()->set_damping( + jointAxis->Data().Damping()); + } } else if (i == 1) { From 64755e578492c0e93a50af55b516aeba9d37d767 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 18 Feb 2022 17:21:28 +0100 Subject: [PATCH 2/2] Added more sensor properties to scene/info topic (#1344) Signed-off-by: ahcorde --- .../scene_broadcaster/SceneBroadcaster.cc | 181 ++++++++++++++++++ 1 file changed, 181 insertions(+) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index e0d7ac0b79..3cf1980d17 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -29,22 +29,40 @@ #include #include +#include "ignition/gazebo/components/AirPressureSensor.hh" +#include "ignition/gazebo/components/Altimeter.hh" +#include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/Imu.hh" +#include "ignition/gazebo/components/LaserRetro.hh" +#include "ignition/gazebo/components/Lidar.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/LogicalCamera.hh" #include "ignition/gazebo/components/Material.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" +#include +#include +#include +#include +#include + using namespace std::chrono_literals; using namespace ignition; @@ -758,6 +776,169 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( sensorMsg->set_name(_nameComp->Data()); sensorMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data())); + auto altimeterComp = _manager.Component(_entity); + if (altimeterComp) + { + sensorMsg->set_type("altimeter"); + } + auto airPressureComp = _manager.Component< + components::AirPressureSensor>(_entity); + if (airPressureComp) + { + sensorMsg->set_type("air_pressure"); + } + auto cameraComp = _manager.Component(_entity); + if (cameraComp) + { + sensorMsg->set_type("camera"); + msgs::CameraSensor * cameraSensorMsg = sensorMsg->mutable_camera(); + const auto * camera = cameraComp->Data().CameraSensor(); + if (camera) + { + cameraSensorMsg->set_horizontal_fov( + camera->HorizontalFov().Radian()); + cameraSensorMsg->mutable_image_size()->set_x(camera->ImageWidth()); + cameraSensorMsg->mutable_image_size()->set_y(camera->ImageHeight()); + cameraSensorMsg->set_image_format(camera->PixelFormatStr()); + cameraSensorMsg->set_near_clip(camera->NearClip()); + cameraSensorMsg->set_far_clip(camera->FarClip()); + msgs::Distortion *distortionMsg = + cameraSensorMsg->mutable_distortion(); + if (distortionMsg) + { + distortionMsg->mutable_center()->set_x( + camera->DistortionCenter().X()); + distortionMsg->mutable_center()->set_y( + camera->DistortionCenter().Y()); + distortionMsg->set_k1(camera->DistortionK1()); + distortionMsg->set_k2(camera->DistortionK2()); + distortionMsg->set_k3(camera->DistortionK3()); + distortionMsg->set_p1(camera->DistortionP1()); + distortionMsg->set_p2(camera->DistortionP2()); + } + } + } + auto contactSensorComp = _manager.Component< + components::ContactSensor>(_entity); + if (contactSensorComp) + { + sensorMsg->set_type("contact_sensor"); + } + auto depthCameraSensorComp = _manager.Component< + components::DepthCamera>(_entity); + if (depthCameraSensorComp) + { + sensorMsg->set_type("depth_camera"); + } + auto gpuLidarComp = _manager.Component(_entity); + if (gpuLidarComp) + { + sensorMsg->set_type("gpu_lidar"); + msgs::LidarSensor * lidarSensorMsg = sensorMsg->mutable_lidar(); + const auto * lidar = gpuLidarComp->Data().LidarSensor(); + + if (lidar && lidarSensorMsg) + { + lidarSensorMsg->set_horizontal_samples( + lidar->HorizontalScanSamples()); + lidarSensorMsg->set_horizontal_resolution( + lidar->HorizontalScanResolution()); + lidarSensorMsg->set_horizontal_min_angle( + lidar->HorizontalScanMinAngle().Radian()); + lidarSensorMsg->set_horizontal_max_angle( + lidar->HorizontalScanMaxAngle().Radian()); + lidarSensorMsg->set_vertical_samples(lidar->VerticalScanSamples()); + lidarSensorMsg->set_vertical_resolution( + lidar->VerticalScanResolution()); + lidarSensorMsg->set_vertical_min_angle( + lidar->VerticalScanMinAngle().Radian()); + lidarSensorMsg->set_vertical_max_angle( + lidar->VerticalScanMaxAngle().Radian()); + lidarSensorMsg->set_range_min(lidar->RangeMin()); + lidarSensorMsg->set_range_max(lidar->RangeMax()); + lidarSensorMsg->set_range_resolution(lidar->RangeResolution()); + msgs::SensorNoise *sensorNoise = lidarSensorMsg->mutable_noise(); + if (sensorNoise) + { + const auto noise = lidar->LidarNoise(); + switch(noise.Type()) + { + case sdf::NoiseType::GAUSSIAN: + sensorNoise->set_type(msgs::SensorNoise::GAUSSIAN); + break; + case sdf::NoiseType::GAUSSIAN_QUANTIZED: + sensorNoise->set_type(msgs::SensorNoise::GAUSSIAN_QUANTIZED); + break; + default: + sensorNoise->set_type(msgs::SensorNoise::NONE); + } + sensorNoise->set_mean(noise.Mean()); + sensorNoise->set_stddev(noise.StdDev()); + sensorNoise->set_bias_mean(noise.BiasMean()); + sensorNoise->set_bias_stddev(noise.BiasStdDev()); + sensorNoise->set_precision(noise.Precision()); + sensorNoise->set_dynamic_bias_stddev(noise.DynamicBiasStdDev()); + sensorNoise->set_dynamic_bias_correlation_time( + noise.DynamicBiasCorrelationTime()); + } + } + } + auto imuComp = _manager.Component(_entity); + if (imuComp) + { + sensorMsg->set_type("imu"); + msgs::IMUSensor * imuMsg = sensorMsg->mutable_imu(); + const auto * imu = imuComp->Data().ImuSensor(); + + ignition::gazebo::set( + imuMsg->mutable_linear_acceleration()->mutable_x_noise(), + imu->LinearAccelerationXNoise()); + ignition::gazebo::set( + imuMsg->mutable_linear_acceleration()->mutable_y_noise(), + imu->LinearAccelerationYNoise()); + ignition::gazebo::set( + imuMsg->mutable_linear_acceleration()->mutable_z_noise(), + imu->LinearAccelerationZNoise()); + ignition::gazebo::set( + imuMsg->mutable_angular_velocity()->mutable_x_noise(), + imu->AngularVelocityXNoise()); + ignition::gazebo::set( + imuMsg->mutable_angular_velocity()->mutable_y_noise(), + imu->AngularVelocityYNoise()); + ignition::gazebo::set( + imuMsg->mutable_angular_velocity()->mutable_z_noise(), + imu->AngularVelocityZNoise()); + } + auto laserRetroComp = _manager.Component< + components::LaserRetro>(_entity); + if (laserRetroComp) + { + sensorMsg->set_type("laser_retro"); + } + auto lidarComp = _manager.Component(_entity); + if (lidarComp) + { + sensorMsg->set_type("lidar"); + } + auto logicalCamera = _manager.Component< + components::LogicalCamera>(_entity); + if (logicalCamera) + { + sensorMsg->set_type("logical_camera"); + } + auto rgbdCameraComp = _manager.Component< + components::RgbdCamera>(_entity); + if (rgbdCameraComp) + { + sensorMsg->set_type("rgbd_camera"); + } + auto thermalCameraComp = _manager.Component< + components::ThermalCamera>(_entity); + if (thermalCameraComp) + { + sensorMsg->set_type("thermal_camera"); + } + // Add to graph newGraph.AddVertex(_nameComp->Data(), sensorMsg, _entity); newGraph.AddEdge({_parentComp->Data(), _entity}, true);