From 211289f60ceadbcd20a91e457592a67fe1e4a2f4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 15 Feb 2022 09:25:28 +0100 Subject: [PATCH 01/18] Fixed light gui component inspector (#1337) Signed-off-by: ahcorde --- src/gui/plugins/component_inspector/Light.qml | 40 +++++++++---------- src/rendering/RenderUtil.cc | 6 +++ 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml index 15881a434a..98cf143e82 100644 --- a/src/gui/plugins/component_inspector/Light.qml +++ b/src/gui/plugins/component_inspector/Light.qml @@ -873,7 +873,7 @@ Rectangle { } RowLayout { Text { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 Layout.columnSpan: 6 text: " Direction" color: "dimgrey" @@ -882,7 +882,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 color: "transparent" height: 40 Layout.preferredWidth: xDirectionText.width + indentation*3 @@ -896,7 +896,7 @@ Rectangle { Component.onCompleted: loaderDirectionX.item.componentInfo = "directionX" Text { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 id : xDirectionText text: ' X:' leftPadding: 5 @@ -906,7 +906,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 @@ -923,7 +923,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 color: "transparent" height: 40 Layout.preferredWidth: yDirectionText.width + indentation*3 @@ -937,7 +937,7 @@ Rectangle { Component.onCompleted: loaderDirectionY.item.componentInfo = "directionY" Text { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 id : yDirectionText text: ' Y:' leftPadding: 5 @@ -947,7 +947,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 @@ -964,7 +964,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 color: "transparent" height: 40 Layout.preferredWidth: zDirectionText.width + indentation*3 @@ -978,7 +978,7 @@ Rectangle { Component.onCompleted: loaderDirectionZ.item.componentInfo = "directionZ" Text { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 id : zDirectionText text: ' Z:' leftPadding: 5 @@ -988,7 +988,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 || model.data[19] === 2 + visible: model.data[20] === 1 || model.data[20] === 2 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 @@ -1005,7 +1005,7 @@ Rectangle { } RowLayout { Text { - visible: model.data[19] === 1 + visible: model.data[20] === 1 Layout.columnSpan: 6 text: " Spot features" color: "dimgrey" @@ -1014,7 +1014,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 + visible: model.data[20] === 1 color: "transparent" height: 40 Layout.preferredWidth: innerAngleText.width + indentation*3 @@ -1028,7 +1028,7 @@ Rectangle { Component.onCompleted: loaderInnerAngle.item.componentInfo = "innerAngle" Text { - visible: model.data[19] === 1 + visible: model.data[20] === 1 id : innerAngleText text: ' Inner Angle:' leftPadding: 5 @@ -1038,7 +1038,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 + visible: model.data[20] === 1 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 @@ -1055,7 +1055,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 + visible: model.data[20] === 1 color: "transparent" height: 40 Layout.preferredWidth: outerAngleText.width + indentation*3 @@ -1069,7 +1069,7 @@ Rectangle { Component.onCompleted: loaderOuterAngle.item.componentInfo = "outerAngle" Text { - visible: model.data[19] === 1 + visible: model.data[20] === 1 id : outerAngleText text: ' Outer angle:' leftPadding: 5 @@ -1079,7 +1079,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 + visible: model.data[20] === 1 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 @@ -1096,7 +1096,7 @@ Rectangle { } RowLayout { Rectangle { - visible: model.data[19] === 1 + visible: model.data[20] === 1 color: "transparent" height: 40 Layout.preferredWidth: fallOffText.width + indentation*3 @@ -1110,7 +1110,7 @@ Rectangle { Component.onCompleted: loaderFallOff.item.componentInfo = "falloff" Text { - visible: model.data[19] === 1 + visible: model.data[20] === 1 id : fallOffText text: ' Falloff:' leftPadding: 5 @@ -1120,7 +1120,7 @@ Rectangle { } } Item { - visible: model.data[19] === 1 + visible: model.data[20] === 1 Layout.fillWidth: true height: 40 Layout.columnSpan: 4 diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index a705df0e5f..35c347cd33 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2131,6 +2131,12 @@ void RenderUtilPrivate::UpdateLights( auto l = std::dynamic_pointer_cast(node); if (l) { + if (!ignition::math::equal( + l->Intensity(), + static_cast(light.second.intensity()))) + { + l->SetIntensity(light.second.intensity()); + } if (light.second.has_diffuse()) { if (l->DiffuseColor() != msgs::Convert(light.second.diffuse())) 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 02/18] 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 03/18] 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); From 8b8b9f6ad5288404fd15818537477644be4b7ffb Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 24 Feb 2022 13:32:43 -0800 Subject: [PATCH 04/18] Support disabling pose publisher from publishing top level model pose (#1342) The pose-publisher system uses only one parameter, publish_nested_model_pose, to determine whether or not top level model and nested model poses should be published. This PR adds another parameter, publish_model_pose, to let users decouple pose publishing behavior between nested models and top level models. Signed-off-by: Ian Chen --- src/systems/pose_publisher/PosePublisher.cc | 32 +++++++++++++++++++-- src/systems/pose_publisher/PosePublisher.hh | 4 ++- test/integration/pose_publisher_system.cc | 15 ++++++++++ test/worlds/nested_model.sdf | 5 ++-- 4 files changed, 50 insertions(+), 6 deletions(-) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 8d550f68f6..6152c7d2a0 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -111,6 +111,9 @@ class ignition::gazebo::systems::PosePublisherPrivate /// \brief True to publish nested model pose public: bool publishNestedModelPose = false; + /// \brief True to publish model pose + public: bool publishModelPose = false; + /// \brief Frequency of pose publications in Hz. A negative frequency /// publishes as fast as possible (i.e, at the rate of the simulation step) public: double updateFrequency = -1; @@ -197,6 +200,12 @@ void PosePublisher::Configure(const Entity &_entity, _sdf->Get("publish_nested_model_pose", this->dataPtr->publishNestedModelPose).first; + // for backward compatibility, publish_model_pose will be set to the + // same value as publish_nested_model_pose if it is not specified. + this->dataPtr->publishModelPose = + _sdf->Get("publish_model_pose", + this->dataPtr->publishNestedModelPose).first; + this->dataPtr->publishVisualPose = _sdf->Get("publish_visual_pose", this->dataPtr->publishVisualPose).first; @@ -364,18 +373,36 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( visited.push_back(entity); auto link = _ecm.Component(entity); - auto nestedModel = _ecm.Component(entity); auto visual = _ecm.Component(entity); auto collision = _ecm.Component(entity); auto sensor = _ecm.Component(entity); auto joint = _ecm.Component(entity); + auto isModel = _ecm.Component(entity); + auto parent = _ecm.Component(entity); + bool fillPose = (link && this->publishLinkPose) || - (nestedModel && this->publishNestedModelPose) || (visual && this->publishVisualPose) || (collision && this->publishCollisionPose) || (sensor && this->publishSensorPose); + // for backward compatibility, top level model pose will be published + // if publishNestedModelPose is set to true unless the user explicity + // disables this by setting publishModelPose to false + if (isModel) + { + if (parent) + { + auto nestedModel = _ecm.Component(parent->Data()); + if (nestedModel) + fillPose = this->publishNestedModelPose; + } + if (!fillPose) + { + fillPose = this->publishNestedModelPose && this->publishModelPose; + } + } + if (fillPose) { std::string frame; @@ -386,7 +413,6 @@ void PosePublisherPrivate::InitializeEntitiesToPublish( childFrame = removeParentScope(scopedName(entity, _ecm, "::", false), "::"); - auto parent = _ecm.Component(entity); if (parent) { auto parentName = _ecm.Component(parent->Data()); diff --git a/src/systems/pose_publisher/PosePublisher.hh b/src/systems/pose_publisher/PosePublisher.hh index abd794ea25..e3797b0bf6 100644 --- a/src/systems/pose_publisher/PosePublisher.hh +++ b/src/systems/pose_publisher/PosePublisher.hh @@ -43,9 +43,11 @@ namespace systems /// publish_visual_pose : Set to true to publish visual pose /// publish_collision_pose : Set to true to publish collision pose /// publish_sensor_pose : Set to true to publish sensor pose + /// publish_model_pose : Set to true to publish model pose. /// publish_nested_model_pose : Set to true to publish nested model pose. The /// pose of the model that contains this system is - /// also published. + /// also published unless publish_model_pose is + /// set to false /// use_pose_vector_msg : Set to true to publish an /// ignition::msgs::Pose_V message instead of /// mulitple ignition::msgs::Pose messages. diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index 0a1a19f3d6..ddb4f131e0 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -742,4 +742,19 @@ TEST_F(PosePublisherTest, } EXPECT_TRUE(!poseMsgs.empty()); + + // only the pose of the nested model should be published and no other entity + std::string expectedEntityName = "model_00::model_01"; + math::Pose3d expectedEntityPose(1, 0, 0, 0, 0, 0); + for (auto &msg : poseMsgs) + { + ASSERT_LT(1, msg.header().data_size()); + ASSERT_LT(0, msg.header().data(1).value_size()); + std::string childFrameId = msg.header().data(1).value(0); + EXPECT_EQ(expectedEntityName, childFrameId); + math::Pose3d pose; + auto p = msgs::Convert(poseMsgs[0]); + EXPECT_EQ(expectedEntityPose, p); + } + } diff --git a/test/worlds/nested_model.sdf b/test/worlds/nested_model.sdf index 06063d67eb..68ab84077e 100644 --- a/test/worlds/nested_model.sdf +++ b/test/worlds/nested_model.sdf @@ -63,11 +63,12 @@ - true + false false false false - false + true + false 100 From d3a5015450828ece46cadc92223dccb7733f9a3e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 24 Feb 2022 16:00:33 -0800 Subject: [PATCH 05/18] Add parameter to TrajectoryFollower stop rotation when bearing is reached (#1349) * force stop rotation by setting ang vel to zero Signed-off-by: Ian Chen * update example and bearing tol Signed-off-by: Ian Chen * update tol check Signed-off-by: Ian Chen * add note Signed-off-by: Ian Chen * remove param from example world Signed-off-by: Ian Chen --- include/ignition/gazebo/Link.hh | 8 +++++ src/Link.cc | 20 +++++++++++ src/systems/physics/Physics.cc | 1 - .../trajectory_follower/TrajectoryFollower.cc | 35 ++++++++++++++++++- .../trajectory_follower/TrajectoryFollower.hh | 5 +++ test/integration/link.cc | 18 ++++++++++ 6 files changed, 85 insertions(+), 2 deletions(-) diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 9dd3d5ccc7..df967d30ca 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -227,6 +227,14 @@ namespace ignition public: void SetLinearVelocity(EntityComponentManager &_ecm, const math::Vector3d &_vel) const; + + /// \brief Set the angular velocity on this link. If this is set, wrenches + /// on this link will be ignored for the current time step. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _vel Angular velocity to set in Link's Frame. + public: void SetAngularVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const; + /// \brief Get the angular acceleration of the body in the world frame. /// \param[in] _ecm Entity-component manager. /// \return Angular acceleration of the body in the world frame or diff --git a/src/Link.cc b/src/Link.cc index 0d07f17140..82d860fdb1 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -19,6 +19,7 @@ #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" #include "ignition/gazebo/components/CanonicalLink.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" @@ -266,6 +267,25 @@ void Link::SetLinearVelocity(EntityComponentManager &_ecm, } } +////////////////////////////////////////////////// +void Link::SetAngularVelocity(EntityComponentManager &_ecm, + const math::Vector3d &_vel) const +{ + auto vel = + _ecm.Component(this->dataPtr->id); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AngularVelocityCmd(_vel)); + } + else + { + vel->Data() = _vel; + } +} + ////////////////////////////////////////////////// std::optional Link::WorldAngularAcceleration( const EntityComponentManager &_ecm) const diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 18df36ba58..bcd6146868 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2150,7 +2150,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) worldAngularVelFeature->SetWorldAngularVelocity( math::eigen3::convert(worldAngularVel)); - return true; }); diff --git a/src/systems/trajectory_follower/TrajectoryFollower.cc b/src/systems/trajectory_follower/TrajectoryFollower.cc index c48ba3d8f9..8366c6065d 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.cc +++ b/src/systems/trajectory_follower/TrajectoryFollower.cc @@ -33,6 +33,7 @@ #include #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" #include "ignition/gazebo/Link.hh" #include "ignition/gazebo/Model.hh" #include "ignition/gazebo/Util.hh" @@ -109,6 +110,12 @@ class ignition::gazebo::systems::TrajectoryFollowerPrivate /// \brief Whether the trajectory follower behavior should be paused or not. public: bool paused = false; + + /// \brief Angular velocity set to zero + public: bool zeroAngVelSet = false; + + /// \brief Force angular velocity to be zero when bearing is reached + public: bool forceZeroAngVel = false; }; ////////////////////////////////////////////////// @@ -249,6 +256,10 @@ void TrajectoryFollowerPrivate::Load(const EntityComponentManager &_ecm, if (_sdf->HasElement("bearing_tolerance")) this->bearingTolerance = _sdf->Get("bearing_tolerance"); + // Parse the optional element. + if (_sdf->HasElement("zero_vel_on_bearing_reached")) + this->forceZeroAngVel = _sdf->Get("zero_vel_on_bearing_reached"); + // Parse the optional element. this->topic = "/model/" + this->model.Name(_ecm) + "/trajectory_follower/pause"; @@ -376,11 +387,33 @@ void TrajectoryFollower::PreUpdate( { forceWorld = (*comPose).Rot().RotateVector( ignition::math::Vector3d(this->dataPtr->forceToApply, 0, 0)); - } + // force angular velocity to be zero when bearing is reached + if (this->dataPtr->forceZeroAngVel && !this->dataPtr->zeroAngVelSet && + math::equal (std::abs(bearing.Degree()), 0.0, + this->dataPtr->bearingTolerance * 0.5)) + { + this->dataPtr->link.SetAngularVelocity(_ecm, math::Vector3d::Zero); + this->dataPtr->zeroAngVelSet = true; + } + } ignition::math::Vector3d torqueWorld; if (std::abs(bearing.Degree()) > this->dataPtr->bearingTolerance) { + // remove angular velocity component otherwise the physics system will set + // the zero ang vel command every iteration + if (this->dataPtr->forceZeroAngVel && this->dataPtr->zeroAngVelSet) + { + auto angVelCmdComp = _ecm.Component( + this->dataPtr->link.Entity()); + if (angVelCmdComp) + { + _ecm.RemoveComponent( + this->dataPtr->link.Entity()); + this->dataPtr->zeroAngVelSet = false; + } + } + int sign = std::abs(bearing.Degree()) / bearing.Degree(); torqueWorld = (*comPose).Rot().RotateVector( ignition::math::Vector3d(0, 0, sign * this->dataPtr->torqueToApply)); diff --git a/src/systems/trajectory_follower/TrajectoryFollower.hh b/src/systems/trajectory_follower/TrajectoryFollower.hh index c1d52aca06..57fbfcaae9 100644 --- a/src/systems/trajectory_follower/TrajectoryFollower.hh +++ b/src/systems/trajectory_follower/TrajectoryFollower.hh @@ -66,6 +66,11 @@ namespace systems /// : If the bearing to the current waypoint is within /// +- this tolerance, a torque won't be applied (degree) /// The default value is 2 deg. + /// : Force angular velocity to be zero when + /// target bearing is reached. + /// Default is false. + /// Note: this is an experimental parameter + /// and may be removed in the future. /// : The force to apply at every plugin iteration in the X direction /// of the link (N). The default value is 60. /// : The torque to apply at every plugin iteration in the Yaw diff --git a/test/integration/link.cc b/test/integration/link.cc index 53a1dc9f7c..d6b70cccf0 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -500,6 +501,23 @@ TEST_F(LinkIntegrationTest, LinkSetVelocity) link.SetLinearVelocity(ecm, linVel2); EXPECT_EQ(linVel2, ecm.Component(eLink)->Data()); + + // No AngularVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLink)); + + math::Vector3d angVel(0, 0, 1); + link.SetAngularVelocity(ecm, angVel); + + // AngularVelocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLink)); + EXPECT_EQ(angVel, + ecm.Component(eLink)->Data()); + + // Make sure the angular velocity is updated + math::Vector3d angVel2(0, 0, 0); + link.SetAngularVelocity(ecm, angVel2); + EXPECT_EQ(angVel2, + ecm.Component(eLink)->Data()); } ////////////////////////////////////////////////// From 0ae1324cec2477da3abda89b14f605d71f3cb29a Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 24 Feb 2022 18:54:07 -0800 Subject: [PATCH 06/18] Fix accessing empty JointPosition component in lift drag plugin (#1366) Check for empty JointControl component before accessing it Signed-off-by: Ian Chen --- src/systems/lift_drag/LiftDrag.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 841858cc55..d809f2292c 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -382,7 +382,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) cl = this->cla * alpha * cosSweepAngle; // modify cl per control joint value - if (controlJointPosition) + if (controlJointPosition && !controlJointPosition->Data().empty()) { cl = cl + this->controlJointRadToCL * controlJointPosition->Data()[0]; /// \todo(anyone): also change cm and cd From 3b94bd2b94ec3551d00e898cec96578677a58812 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 24 Feb 2022 19:00:58 -0800 Subject: [PATCH 07/18] Prepare for 6.6.0 release (#1365) * prepare for 6.6.0 release Signed-off-by: Ian Chen * update changelog Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 38 ++++++++++++++++++++++++++++++++++++-- 2 files changed, 37 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eaf570f512..8823afa320 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.5.0) +project(ignition-gazebo6 VERSION 6.6.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 36bfd47980..5a1ae8c3d3 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,8 +1,42 @@ ## Ignition Gazebo 6.x -### Ignition Gazebo 6.X.X (202X-XX-XX) +### Ignition Gazebo 6.6.0 (2022-02-24) + +1. Fix accessing empty JointPosition component in lift drag plugin + * [Pull request #1366](https://github.com/ignitionrobotics/ign-gazebo/pull/1366) + +1. Add parameter to TrajectoryFollower stop rotation when bearing is reached + * [Pull request #1349](https://github.com/ignitionrobotics/ign-gazebo/pull/1349) + +1. Support disabling pose publisher from publishing top level model pose + * [Pull request #1342](https://github.com/ignitionrobotics/ign-gazebo/pull/1342) + +1. Added more sensor properties to scene/info topic + * [Pull request #1344](https://github.com/ignitionrobotics/ign-gazebo/pull/1344) + +1. Adding ability to pause/resume the trajectory follower behavior. + * [Pull request #1347](https://github.com/ignitionrobotics/ign-gazebo/pull/1347) + +1. Logs a warning if a mode is not clearly sepecified. + * [Pull request #1307](https://github.com/ignitionrobotics/ign-gazebo/pull/1307) + +1. JointStatePublisher publish parent, child and axis data + * [Pull request #1345](https://github.com/ignitionrobotics/ign-gazebo/pull/1345) + +1. Fixed light gui component inspector + * [Pull request #1337](https://github.com/ignitionrobotics/ign-gazebo/pull/1337) + +1. Fix UNIT_SdfGenerator_TEST + * [Pull request #1319](https://github.com/ignitionrobotics/ign-gazebo/pull/1319) + +1. Add elevator system + * [Pull request #535](https://github.com/ignitionrobotics/ign-gazebo/pull/535) + +1. Removed unused variables in shapes plugin + * [Pull request #1321](https://github.com/ignitionrobotics/ign-gazebo/pull/1321) + +### Ignition Gazebo 6.5.0 (2022-02-15) -### Ignition Gazebo 6.5.0 (202X-XX-XX) 1. New trajectory follower system * [Pull request #1332](https://github.com/ignitionrobotics/ign-gazebo/pull/1332) From ac989f8bab5a567a0243e25fab38d38557b774e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 25 Feb 2022 20:22:55 +0100 Subject: [PATCH 08/18] Added Python interfaces to some Ignition Gazebo methods (#1219) Signed-off-by: ahcorde Co-authored-by: Louise Poubel Co-authored-by: Jose Luis Rivero --- .github/ci/packages-focal.apt | 1 + .github/ci/packages.apt | 2 + CMakeLists.txt | 32 ++ examples/scripts/python_api/README.md | 6 + examples/scripts/python_api/gravity.sdf | 22 ++ examples/scripts/python_api/testFixture.py | 85 +++++ python/CMakeLists.txt | 99 ++++++ python/src/ignition/common/Console.cc | 31 ++ python/src/ignition/common/Console.hh | 31 ++ .../common/_ignition_common_pybind11.cc | 25 ++ .../ignition/gazebo/EntityComponentManager.cc | 35 ++ .../ignition/gazebo/EntityComponentManager.hh | 40 +++ python/src/ignition/gazebo/EventManager.cc | 37 ++ python/src/ignition/gazebo/EventManager.hh | 39 +++ python/src/ignition/gazebo/Server.cc | 52 +++ python/src/ignition/gazebo/Server.hh | 39 +++ python/src/ignition/gazebo/ServerConfig.cc | 40 +++ python/src/ignition/gazebo/ServerConfig.hh | 39 +++ python/src/ignition/gazebo/TestFixture.cc | 103 ++++++ python/src/ignition/gazebo/TestFixture.hh | 38 +++ python/src/ignition/gazebo/UpdateInfo.cc | 43 +++ python/src/ignition/gazebo/UpdateInfo.hh | 39 +++ python/src/ignition/gazebo/Util.cc | 41 +++ python/src/ignition/gazebo/Util.hh | 37 ++ python/src/ignition/gazebo/World.cc | 45 +++ python/src/ignition/gazebo/World.hh | 40 +++ .../gazebo/_ignition_gazebo_pybind11.cc | 39 +++ python/src/ignition/gazebo/wrap_functions.hh | 323 ++++++++++++++++++ python/src/ignition/sdformat/Element.cc | 36 ++ python/src/ignition/sdformat/Element.hh | 38 +++ .../ignition/sdformat/_sdformat_pybind11.cc | 23 ++ python/test/gravity.sdf | 22 ++ python/test/testFixture_TEST.py | 69 ++++ tutorials.md.in | 1 + tutorials/python_interfaces.md | 82 +++++ 35 files changed, 1674 insertions(+) create mode 100644 examples/scripts/python_api/README.md create mode 100644 examples/scripts/python_api/gravity.sdf create mode 100644 examples/scripts/python_api/testFixture.py create mode 100644 python/CMakeLists.txt create mode 100644 python/src/ignition/common/Console.cc create mode 100644 python/src/ignition/common/Console.hh create mode 100644 python/src/ignition/common/_ignition_common_pybind11.cc create mode 100644 python/src/ignition/gazebo/EntityComponentManager.cc create mode 100644 python/src/ignition/gazebo/EntityComponentManager.hh create mode 100644 python/src/ignition/gazebo/EventManager.cc create mode 100644 python/src/ignition/gazebo/EventManager.hh create mode 100644 python/src/ignition/gazebo/Server.cc create mode 100644 python/src/ignition/gazebo/Server.hh create mode 100644 python/src/ignition/gazebo/ServerConfig.cc create mode 100644 python/src/ignition/gazebo/ServerConfig.hh create mode 100644 python/src/ignition/gazebo/TestFixture.cc create mode 100644 python/src/ignition/gazebo/TestFixture.hh create mode 100644 python/src/ignition/gazebo/UpdateInfo.cc create mode 100644 python/src/ignition/gazebo/UpdateInfo.hh create mode 100644 python/src/ignition/gazebo/Util.cc create mode 100644 python/src/ignition/gazebo/Util.hh create mode 100644 python/src/ignition/gazebo/World.cc create mode 100644 python/src/ignition/gazebo/World.hh create mode 100644 python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc create mode 100644 python/src/ignition/gazebo/wrap_functions.hh create mode 100644 python/src/ignition/sdformat/Element.cc create mode 100644 python/src/ignition/sdformat/Element.hh create mode 100644 python/src/ignition/sdformat/_sdformat_pybind11.cc create mode 100644 python/test/gravity.sdf create mode 100644 python/test/testFixture_TEST.py create mode 100644 tutorials/python_interfaces.md diff --git a/.github/ci/packages-focal.apt b/.github/ci/packages-focal.apt index 50cee478b6..fe94da46f7 100644 --- a/.github/ci/packages-focal.apt +++ b/.github/ci/packages-focal.apt @@ -3,3 +3,4 @@ libdart-dev libdart-external-ikfast-dev libdart-external-odelcpsolver-dev libdart-utils-urdf-dev +python3-ignition-math6 diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 4a15aa2ddf..5308887a5e 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -22,6 +22,8 @@ libsdformat12-dev libtinyxml2-dev libxi-dev libxmu-dev +python3-distutils +python3-pybind11 qml-module-qt-labs-folderlistmodel qml-module-qt-labs-settings qml-module-qtqml-models2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 8823afa320..0172ce3b42 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ignition-cmake2 2.8.0 REQUIRED) # Configure the project #============================================================================ ign_configure_project(VERSION_SUFFIX) +set (CMAKE_CXX_STANDARD 17) #============================================================================ # Set project-specific options @@ -37,6 +38,14 @@ endif() include(test/find_dri.cmake) FindDRI() +option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION + "Install python modules in standard system paths in the system" + OFF) + +option(USE_DIST_PACKAGES_FOR_PYTHON + "Use dist-packages instead of site-package to install python modules" + OFF) + #============================================================================ # Search for project-specific dependencies #============================================================================ @@ -167,6 +176,26 @@ ign_find_package(IgnProtobuf PRETTY Protobuf) set(Protobuf_IMPORT_DIRS ${ignition-msgs8_INCLUDE_DIRS}) +#-------------------------------------- +# Find python +include(IgnPython) +find_package(PythonLibs QUIET) +if (NOT PYTHONLIBS_FOUND) + IGN_BUILD_WARNING("Python is missing: Python interfaces are disabled.") + message (STATUS "Searching for Python - not found.") +else() + message (STATUS "Searching for Python - found version ${PYTHONLIBS_VERSION_STRING}.") + + set(PYBIND11_PYTHON_VERSION 3) + find_package(pybind11 2.2 QUIET) + + if (${pybind11_FOUND}) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() +endif() # Plugin install dirs set(IGNITION_GAZEBO_PLUGIN_INSTALL_DIR ${CMAKE_INSTALL_PREFIX}/${IGN_LIB_INSTALL_DIR}/ign-${IGN_DESIGNATION}-${PROJECT_VERSION_MAJOR}/plugins @@ -187,6 +216,9 @@ add_subdirectory(examples) #============================================================================ ign_create_packages() +if (${pybind11_FOUND}) + add_subdirectory(python) +endif() #============================================================================ # Configure documentation #============================================================================ diff --git a/examples/scripts/python_api/README.md b/examples/scripts/python_api/README.md new file mode 100644 index 0000000000..a828853737 --- /dev/null +++ b/examples/scripts/python_api/README.md @@ -0,0 +1,6 @@ +# Gazebo Python API + +This example shows how to use Gazebo's Python API. + +For more information, see the +[Python interfaces](https://ignitionrobotics.org/api/gazebo/6.4/python_interfaces.html) tutorial. diff --git a/examples/scripts/python_api/gravity.sdf b/examples/scripts/python_api/gravity.sdf new file mode 100644 index 0000000000..ee1161fa5f --- /dev/null +++ b/examples/scripts/python_api/gravity.sdf @@ -0,0 +1,22 @@ + + + + + + + + + + + 0.4 + 0.4 + 0.4 + + 1.0 + + + + + diff --git a/examples/scripts/python_api/testFixture.py b/examples/scripts/python_api/testFixture.py new file mode 100644 index 0000000000..caedebdc84 --- /dev/null +++ b/examples/scripts/python_api/testFixture.py @@ -0,0 +1,85 @@ +#!/usr/bin/python3 +# Copyright (C) 2021 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# If you compiled Ignition Gazebo from source you should modify your +# `PYTHONPATH`: +# +# export PYTHONPATH=$PYTHONPATH:/install/lib/python +# +# Now you can run the example: +# +# python3 examples/scripts/python_api/helperFixture.py + +import os +import time + +from ignition.common import set_verbosity +from ignition.gazebo import TestFixture, World, world_entity +from ignition.math import Vector3d +from sdformat import Element + +set_verbosity(4) + +file_path = os.path.dirname(os.path.realpath(__file__)) + +helper = TestFixture(os.path.join(file_path, 'gravity.sdf')) + +post_iterations = 0 +iterations = 0 +pre_iterations = 0 +first_iteration = True + + +def on_pre_udpate_cb(_info, _ecm): + global pre_iterations + global first_iteration + pre_iterations += 1 + if first_iteration: + first_iteration = False + world_e = world_entity(_ecm); + print('World entity is ', world_e) + w = World(world_e) + v = w.gravity(_ecm) + print('Gravity ', v) + modelEntity = w.model_by_name(_ecm, 'falling') + print('Entity for falling model is: ', modelEntity) + + +def on_udpate_cb(_info, _ecm): + global iterations + iterations += 1 + + +def on_post_udpate_cb(_info, _ecm): + global post_iterations + post_iterations += 1 + if _info.sim_time.seconds == 1: + print('Post update sim time: ', _info.sim_time) + + +helper.on_post_update(on_post_udpate_cb) +helper.on_update(on_udpate_cb) +helper.on_pre_update(on_pre_udpate_cb) +helper.finalize() + +server = helper.server() +server.run(False, 1000, False) + +while(server.is_running()): + time.sleep(0.1) + +print('iterations ', iterations) +print('post_iterations ', post_iterations) +print('pre_iterations ', pre_iterations) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000000..bd08e206db --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,99 @@ +if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") + # pybind11 logic for setting up a debug build when both a debug and release + # python interpreter are present in the system seems to be pretty much broken. + # This works around the issue. + set(PYTHON_LIBRARIES "${PYTHON_DEBUG_LIBRARIES}") +endif() + + +if(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION) + if(${CMAKE_VERSION} VERSION_LESS "3.12.0") + execute_process( + COMMAND "${PYTHON_EXECUTABLE}" -c "if True: + from distutils import sysconfig as sc + print(sc.get_python_lib(plat_specific=True))" + OUTPUT_VARIABLE Python3_SITEARCH + OUTPUT_STRIP_TRAILING_WHITESPACE) + else() + # Get install variable from Python3 module + # Python3_SITEARCH is available from 3.12 on, workaround if needed: + find_package(Python3 COMPONENTS Interpreter) + endif() + + if(USE_DIST_PACKAGES_FOR_PYTHON) + string(REPLACE "site-packages" "dist-packages" IGN_PYTHON_INSTALL_PATH ${Python3_SITEARCH}) + endif() +else() + # If not a system installation, respect local paths + set(IGN_PYTHON_INSTALL_PATH ${IGN_LIB_INSTALL_DIR}/python) +endif() + +# Set the build location and install location for a CPython extension +function(configure_build_install_location _library_name) + # Install library for actual use + install(TARGETS ${_library_name} + DESTINATION "${IGN_PYTHON_INSTALL_PATH}/ignition" + ) +endfunction() + +pybind11_add_module(gazebo SHARED + src/ignition/gazebo/_ignition_gazebo_pybind11.cc + src/ignition/gazebo/EntityComponentManager.cc + src/ignition/gazebo/EventManager.cc + src/ignition/gazebo/TestFixture.cc + src/ignition/gazebo/Server.cc + src/ignition/gazebo/ServerConfig.cc + src/ignition/gazebo/UpdateInfo.cc + src/ignition/gazebo/Util.cc + src/ignition/gazebo/World.cc +) + +target_link_libraries(gazebo PRIVATE + ${PROJECT_LIBRARY_TARGET_NAME} + sdformat${SDF_VER}::sdformat${SDF_VER} + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) + +# TODO(ahcorde): Move this module to ign-common +pybind11_add_module(common SHARED + src/ignition/common/_ignition_common_pybind11.cc + src/ignition/common/Console.cc +) + +target_link_libraries(common PRIVATE + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) + +# TODO(ahcorde): Move this module to sdformat +pybind11_add_module(sdformat SHARED + src/ignition/sdformat/_sdformat_pybind11.cc + src/ignition/sdformat/Element.cc +) + +target_link_libraries(sdformat PRIVATE + sdformat${SDF_VER}::sdformat${SDF_VER} +) + +install(TARGETS sdformat + DESTINATION "${IGN_PYTHON_INSTALL_PATH}" +) + +configure_build_install_location(gazebo) +configure_build_install_location(common) + +if (BUILD_TESTING) + set(python_tests + testFixture_TEST + ) + + foreach (test ${python_tests}) + add_test(NAME ${test}.py COMMAND + "${PYTHON_EXECUTABLE}" "${CMAKE_SOURCE_DIR}/python/test/${test}.py") + + set(_env_vars) + list(APPEND _env_vars "PYTHONPATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/python/") + list(APPEND _env_vars "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}:$ENV{LD_LIBRARY_PATH}") + set_tests_properties(${test}.py PROPERTIES + ENVIRONMENT "${_env_vars}") + endforeach() +endif() diff --git a/python/src/ignition/common/Console.cc b/python/src/ignition/common/Console.cc new file mode 100644 index 0000000000..8d904a4487 --- /dev/null +++ b/python/src/ignition/common/Console.cc @@ -0,0 +1,31 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "Console.hh" + +namespace ignition +{ + namespace common + { + namespace python + { + void SetVerbosity(int _verbosity) + { + ignition::common::Console::SetVerbosity(_verbosity); + } + } + } +} diff --git a/python/src/ignition/common/Console.hh b/python/src/ignition/common/Console.hh new file mode 100644 index 0000000000..4b84fbf514 --- /dev/null +++ b/python/src/ignition/common/Console.hh @@ -0,0 +1,31 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef IGNITION_GAZEBO_PYTHON__CONSOLE_HH_ +#define IGNITION_GAZEBO_PYTHON__CONSOLE_HH_ + +#include + +namespace ignition +{ + namespace common + { + namespace python + { + void SetVerbosity(int _verbosity); + } + } +} + +#endif diff --git a/python/src/ignition/common/_ignition_common_pybind11.cc b/python/src/ignition/common/_ignition_common_pybind11.cc new file mode 100644 index 0000000000..12a67670fc --- /dev/null +++ b/python/src/ignition/common/_ignition_common_pybind11.cc @@ -0,0 +1,25 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "Console.hh" + +PYBIND11_MODULE(common, m) { + m.doc() = "Ignition Common Python Library."; + + m.def( + "set_verbosity", &ignition::common::python::SetVerbosity, + "Set verbosity level."); +} diff --git a/python/src/ignition/gazebo/EntityComponentManager.cc b/python/src/ignition/gazebo/EntityComponentManager.cc new file mode 100644 index 0000000000..1786ba387c --- /dev/null +++ b/python/src/ignition/gazebo/EntityComponentManager.cc @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include + +#include "EntityComponentManager.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +///////////////////////////////////////////////// +void defineGazeboEntityComponentManager(pybind11::object module) +{ + pybind11::class_( + module, "EntityComponentManager") + .def(pybind11::init<>()); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/EntityComponentManager.hh b/python/src/ignition/gazebo/EntityComponentManager.hh new file mode 100644 index 0000000000..b982483631 --- /dev/null +++ b/python/src/ignition/gazebo/EntityComponentManager.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ +#define IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::EntityComponentManager +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboEntityComponentManager(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__ENTITY_COMPONENT_MANAGER_HH_ diff --git a/python/src/ignition/gazebo/EventManager.cc b/python/src/ignition/gazebo/EventManager.cc new file mode 100644 index 0000000000..39ecbfb8b5 --- /dev/null +++ b/python/src/ignition/gazebo/EventManager.cc @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "ignition/gazebo/EventManager.hh" + +#include "EventManager.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +///////////////////////////////////////////////// +void defineGazeboEventManager(pybind11::object module) +{ + pybind11::class_(module, "EventManager") + .def(pybind11::init<>()); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/EventManager.hh b/python/src/ignition/gazebo/EventManager.hh new file mode 100644 index 0000000000..bcd177edcb --- /dev/null +++ b/python/src/ignition/gazebo/EventManager.hh @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ +#define IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::EventManager +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboEventManager(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__EVENT_MANAGER_HH_ diff --git a/python/src/ignition/gazebo/Server.cc b/python/src/ignition/gazebo/Server.cc new file mode 100644 index 0000000000..917560a8ae --- /dev/null +++ b/python/src/ignition/gazebo/Server.cc @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include + +#include +#include + +#include "Server.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void defineGazeboServer(pybind11::object module) +{ + pybind11::class_>(module, "Server") + .def(pybind11::init()) + .def( + "run", &ignition::gazebo::Server::Run, + "Run the server. By default this is a non-blocking call, " + " which means the server runs simulation in a separate thread. Pass " + " in true to the _blocking argument to run the server in the current " + " thread.") + .def( + "has_entity", &ignition::gazebo::Server::HasEntity, + "Return true if the specified world has an entity with the provided name.") + .def( + "is_running", + pybind11::overload_cast<>(&ignition::gazebo::Server::Running, pybind11::const_), + "Get whether the server is running."); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/Server.hh b/python/src/ignition/gazebo/Server.hh new file mode 100644 index 0000000000..6e969a035b --- /dev/null +++ b/python/src/ignition/gazebo/Server.hh @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef IGNITION_GAZEBO_PYTHON__SERVER_HH_ +#define IGNITION_GAZEBO_PYTHON__SERVER_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::Server +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboServer(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_HH_ diff --git a/python/src/ignition/gazebo/ServerConfig.cc b/python/src/ignition/gazebo/ServerConfig.cc new file mode 100644 index 0000000000..d8b650aff1 --- /dev/null +++ b/python/src/ignition/gazebo/ServerConfig.cc @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include + +#include + +#include "ServerConfig.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void defineGazeboServerConfig(pybind11::object module) +{ + pybind11::class_(module, "ServerConfig") + .def(pybind11::init<>()) + .def( + "set_sdf_file", &ignition::gazebo::ServerConfig::SetSdfFile, + "Set an SDF file to be used with the server."); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/ServerConfig.hh b/python/src/ignition/gazebo/ServerConfig.hh new file mode 100644 index 0000000000..d955bd5f49 --- /dev/null +++ b/python/src/ignition/gazebo/ServerConfig.hh @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ +#define IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::ServerConfig +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboServerConfig(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__SERVER_CONFIG_HH_ diff --git a/python/src/ignition/gazebo/TestFixture.cc b/python/src/ignition/gazebo/TestFixture.cc new file mode 100644 index 0000000000..7be546716a --- /dev/null +++ b/python/src/ignition/gazebo/TestFixture.cc @@ -0,0 +1,103 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include + +#include "TestFixture.hh" + +#include "ignition/gazebo/TestFixture.hh" + +#include "wrap_functions.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void +defineGazeboTestFixture(pybind11::object module) +{ + pybind11::class_> testFixture(module, "TestFixture"); + + testFixture + .def(pybind11::init()) + .def( + "server", &TestFixture::Server, + pybind11::return_value_policy::reference, + "Get pointer to underlying server." + ) + .def( + "finalize", &TestFixture::Finalize, + pybind11::return_value_policy::reference, + "Finalize all the functions and add fixture to server." + ) + .def( + "on_pre_update", WrapCallbacks( + [](TestFixture* self, std::function _cb) + { + self->OnPreUpdate(_cb); + } + ), + pybind11::return_value_policy::reference, + "Wrapper around a system's pre-update callback" + ) + .def( + "on_update", WrapCallbacks( + [](TestFixture* self, std::function _cb) + { + self->OnUpdate(_cb); + } + ), + pybind11::return_value_policy::reference, + "Wrapper around a system's update callback" + ) + .def( + "on_post_update", WrapCallbacks( + [](TestFixture* self, std::function _cb) + { + self->OnPostUpdate(_cb); + } + ), + pybind11::return_value_policy::reference, + "Wrapper around a system's post-update callback" + ); + // TODO(ahcorde): This method is not compiling for the following reason: + // The EventManager class has an unordered_map which holds a unique_ptr + // This make the class uncopyable, anyhow we should not copy the class + // we just need the reference. Not really sure about what's going on here + // .def( + // "on_configure", WrapCallbacks( + // [](TestFixture* self, std::function &_sdf, + // EntityComponentManager &_ecm, + // EventManager &_eventMgr)> _cb) + // { + // self->OnConfigure(_cb); + // } + // ), + // pybind11::return_value_policy::reference, + // "Wrapper around a system's configure callback" + // ); +} +} +} +} diff --git a/python/src/ignition/gazebo/TestFixture.hh b/python/src/ignition/gazebo/TestFixture.hh new file mode 100644 index 0000000000..b355521943 --- /dev/null +++ b/python/src/ignition/gazebo/TestFixture.hh @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_ +#define IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::TestFixture +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboTestFixture(pybind11::object module); +} +} +} + +#endif // IGNITION_GAZEBO_PYTHON__TEST_FIXTURE_HH_ diff --git a/python/src/ignition/gazebo/UpdateInfo.cc b/python/src/ignition/gazebo/UpdateInfo.cc new file mode 100644 index 0000000000..e31dc7cb92 --- /dev/null +++ b/python/src/ignition/gazebo/UpdateInfo.cc @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include + +#include + +#include "UpdateInfo.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void defineGazeboUpdateInfo(pybind11::object module) +{ + pybind11::class_(module, "UpdateInfo") + .def(pybind11::init<>()) + .def_readwrite("sim_time", &ignition::gazebo::UpdateInfo::simTime) + .def_readwrite("real_time", &ignition::gazebo::UpdateInfo::realTime) + .def_readwrite("dt", &ignition::gazebo::UpdateInfo::dt) + .def_readwrite("paused", &ignition::gazebo::UpdateInfo::paused) + .def_readwrite("iterations", &ignition::gazebo::UpdateInfo::iterations); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/UpdateInfo.hh b/python/src/ignition/gazebo/UpdateInfo.hh new file mode 100644 index 0000000000..f2ec4e910d --- /dev/null +++ b/python/src/ignition/gazebo/UpdateInfo.hh @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ +#define IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::UpdateInfo +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboUpdateInfo(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__UPDATE_INFO_HH_ diff --git a/python/src/ignition/gazebo/Util.cc b/python/src/ignition/gazebo/Util.cc new file mode 100644 index 0000000000..3dd225f8a7 --- /dev/null +++ b/python/src/ignition/gazebo/Util.cc @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include + +#include + +#include +#include "Util.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void defineGazeboUtil(pybind11::module &_module) +{ + _module.def("world_entity", + pybind11::overload_cast( + &ignition::gazebo::worldEntity), + "Get the first world entity that's found."); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/Util.hh b/python/src/ignition/gazebo/Util.hh new file mode 100644 index 0000000000..34db116688 --- /dev/null +++ b/python/src/ignition/gazebo/Util.hh @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_PYTHON__UTIL_HH_ +#define IGNITION_GAZEBO_PYTHON__UTIL_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for a ignition::gazebo::Util +/// \param[in] _module a pybind11 module to add the definition to +void defineGazeboUtil(pybind11::module &_module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__WORLD_HH_ diff --git a/python/src/ignition/gazebo/World.cc b/python/src/ignition/gazebo/World.cc new file mode 100644 index 0000000000..6822c12f8f --- /dev/null +++ b/python/src/ignition/gazebo/World.cc @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#include +#include + +#include + +#include "World.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +void defineGazeboWorld(pybind11::object module) +{ + pybind11::class_(module, "World") + .def(pybind11::init()) + .def( + "model_by_name", &ignition::gazebo::World::ModelByName, + "Get the ID of a model entity which is an immediate child of " + " this world.") + .def( + "gravity", &ignition::gazebo::World::Gravity, + "Get the gravity in m/s^2."); +} +} // namespace python +} // namespace gazebo +} // namespace ignition diff --git a/python/src/ignition/gazebo/World.hh b/python/src/ignition/gazebo/World.hh new file mode 100644 index 0000000000..e77e42b4b6 --- /dev/null +++ b/python/src/ignition/gazebo/World.hh @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_PYTHON__WORLD_HH_ +#define IGNITION_GAZEBO_PYTHON__WORLD_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::gazebo::World +/** + * \param[in] module a pybind11 module to add the definition to + */ +void +defineGazeboWorld(pybind11::object module); +} // namespace python +} // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PYTHON__WORLD_HH_ diff --git a/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc new file mode 100644 index 0000000000..2444a16f02 --- /dev/null +++ b/python/src/ignition/gazebo/_ignition_gazebo_pybind11.cc @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "EntityComponentManager.hh" +#include "EventManager.hh" +#include "Server.hh" +#include "ServerConfig.hh" +#include "TestFixture.hh" +#include "UpdateInfo.hh" +#include "Util.hh" +#include "World.hh" + +PYBIND11_MODULE(gazebo, m) { + m.doc() = "Ignition Gazebo Python Library."; + + ignition::gazebo::python::defineGazeboEntityComponentManager(m); + ignition::gazebo::python::defineGazeboEventManager(m); + ignition::gazebo::python::defineGazeboServer(m); + ignition::gazebo::python::defineGazeboServerConfig(m); + ignition::gazebo::python::defineGazeboTestFixture(m); + ignition::gazebo::python::defineGazeboUpdateInfo(m); + ignition::gazebo::python::defineGazeboWorld(m); + ignition::gazebo::python::defineGazeboUtil(m); +} diff --git a/python/src/ignition/gazebo/wrap_functions.hh b/python/src/ignition/gazebo/wrap_functions.hh new file mode 100644 index 0000000000..2a60b93668 --- /dev/null +++ b/python/src/ignition/gazebo/wrap_functions.hh @@ -0,0 +1,323 @@ +// This code from copied from: +// https://github.com/RobotLocomotion/drake/blob/6ee5e9325821277a62bd5cd5456ccf02ca25dab7/bindings/pydrake/common/wrap_function.h +// It's under BSD 3-Clause License + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include + +namespace internal { + +// Collects both a functor object and its signature for ease of inference. +template +struct function_info { + // TODO(eric.cousineau): Ensure that this permits copy elision when combined + // with `std::forward(func)`, while still behaving well with primitive + // types. + std::decay_t func; +}; + +// Factory method for `function_info<>`, to be used by `infer_function_info`. +template +auto make_function_info(Func&& func, Return (*infer)(Args...) = nullptr) { + (void)infer; + return function_info{std::forward(func)}; +} + +// SFINAE for functors. +// N.B. This *only* distinguished between function / method pointers and +// lambda objects. It does *not* distinguish among other types. +template +using enable_if_lambda_t = + std::enable_if_t>::value, T>; + +// Infers `function_info<>` from a function pointer. +template +auto infer_function_info(Return (*func)(Args...)) { + return make_function_info(func); +} + +// Infers `function_info<>` from a mutable method pointer. +template +auto infer_function_info(Return (Class::*method)(Args...)) { + auto func = [method](Class* self, Args... args) { + return (self->*method)(std::forward(args)...); + }; + return make_function_info(func); +} + +// Infers `function_info<>` from a const method pointer. +template +auto infer_function_info(Return (Class::*method)(Args...) const) { + auto func = [method](const Class* self, Args... args) { + return (self->*method)(std::forward(args)...); + }; + return make_function_info(func); +} + +// Helpers for general functor objects. +struct functor_helpers { + // Removes class from mutable method pointer for inferring signature + // of functor. + template + static auto remove_class_from_ptr(Return (Class::*)(Args...)) { + using Ptr = Return (*)(Args...); + return Ptr{}; + } + + // Removes class from const method pointer for inferring signature of functor. + template + static auto remove_class_from_ptr(Return (Class::*)(Args...) const) { + using Ptr = Return (*)(Args...); + return Ptr{}; + } + + // Infers function pointer from functor. + // @pre `Func` must have only *one* overload of `operator()`. + template + static auto infer_function_ptr() { + return remove_class_from_ptr(&Func::operator()); + } +}; + +// Infers `function_info<>` from a generic functor. +template > +auto infer_function_info(Func&& func) { + return make_function_info(std::forward(func), + functor_helpers::infer_function_ptr>()); +} + +// Implementation for wrapping a function by scanning and replacing arguments +// based on their types. +template