Add collision to the car in Visual lander worlds

CustomRobots/CMakeLists.txt

@@ -36,6 +36,7 @@ foreach(dependency ${PROJECT_DEPENDENCIES})
 endforeach()
 
 add_subdirectory(follow_person_harmonic)
+add_subdirectory(waypoint_follower_harmonic)
 
 # ============================================================================
 # Person plugin

CustomRobots/waypoint_follower_harmonic/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(waypoint_follower_harmonic)
+
+find_package(gz-sim8 REQUIRED)
+find_package(gz-plugin2 REQUIRED)
+find_package(gz-math7 REQUIRED)
+
+add_library(waypoint_follower_harmonic SHARED src/waypoint_follower.cpp)
+
+set_target_properties(waypoint_follower_harmonic PROPERTIES
+  CXX_STANDARD 17
+  CXX_STANDARD_REQUIRED ON)
+
+target_link_libraries(waypoint_follower_harmonic
+  gz-sim8::gz-sim8
+  gz-plugin2::gz-plugin2
+  gz-math7::gz-math7)
+
+install(
+  TARGETS waypoint_follower_harmonic
+  LIBRARY DESTINATION lib
+)

CustomRobots/waypoint_follower_harmonic/src/waypoint_follower.cpp

@@ -0,0 +1,208 @@
+#include <gz/sim/System.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/Util.hh>
+#include <gz/sim/components/AngularVelocityCmd.hh>
+#include <gz/sim/components/LinearVelocityCmd.hh>
+#include <gz/plugin/Register.hh>
+
+#include <gz/math/Helpers.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/Quaternion.hh>
+#include <gz/math/Vector3.hh>
+
+#include <sdf/Element.hh>
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <iostream>
+#include <iterator>
+#include <vector>
+
+// Moves a (non static) model along a list of timed waypoints, like the
+// <script><trajectory> of an actor. Unlike actors, the model keeps its
+// collisions, so other models (e.g. a drone) can land on it and be carried.
+//
+// The motion is applied through velocity commands instead of teleporting, so
+// objects resting on the model get the right contact velocity.
+//
+// <plugin filename="libwaypoint_follower_harmonic.so"
+//         name="waypoint_follower::WaypointFollower">
+//   <loop>true</loop>
+//   <waypoint><time>0.0</time><pose>-12 0 0 0 0 1.5707</pose></waypoint>
+//   <waypoint><time>34.3</time><pose>12 0 0 0 0 1.5707</pose></waypoint>
+// </plugin>
+namespace waypoint_follower
+{
+struct Waypoint
+{
+    double time;
+    gz::math::Pose3d pose;
+};
+
+class WaypointFollower :
+    public gz::sim::System,
+    public gz::sim::ISystemConfigure,
+    public gz::sim::ISystemPreUpdate
+{
+private:
+    gz::sim::Model model{gz::sim::kNullEntity};
+    std::vector<Waypoint> waypoints;
+    bool loop{true};
+
+    // If the model is further than this from its target it is teleported
+    // instead of being driven (e.g. at startup or after a reset)
+    double teleport_distance{1.0};
+
+public:
+    // ============================================
+    // CONFIGURE
+    // ============================================
+    void Configure(const gz::sim::Entity &_entity,
+                   const std::shared_ptr<const sdf::Element> &_sdf,
+                   gz::sim::EntityComponentManager &_ecm,
+                   gz::sim::EventManager &) override
+    {
+        this->model = gz::sim::Model(_entity);
+
+        if (!this->model.Valid(_ecm))
+        {
+            std::cerr << "[WaypointFollower] Plugin must be attached to a model\n";
+            return;
+        }
+
+        if (_sdf->HasElement("loop"))
+            this->loop = _sdf->Get<bool>("loop");
+
+        if (_sdf->HasElement("teleport_distance"))
+            this->teleport_distance = _sdf->Get<double>("teleport_distance");
+
+        for (auto wp = _sdf->FindElement("waypoint"); wp;
+             wp = wp->GetNextElement("waypoint"))
+        {
+            this->waypoints.push_back(
+                {wp->Get<double>("time"), wp->Get<gz::math::Pose3d>("pose")});
+        }
+
+        std::sort(this->waypoints.begin(), this->waypoints.end(),
+                  [](const Waypoint &_a, const Waypoint &_b)
+                  { return _a.time < _b.time; });
+
+        if (this->waypoints.size() < 2)
+        {
+            std::cerr << "[WaypointFollower] At least 2 waypoints are needed\n";
+            this->waypoints.clear();
+        }
+    }
+
+    // ============================================
+    // UPDATE
+    // ============================================
+    void PreUpdate(const gz::sim::UpdateInfo &_info,
+                   gz::sim::EntityComponentManager &_ecm) override
+    {
+        if (_info.paused || this->waypoints.empty() ||
+            !this->model.Valid(_ecm))
+            return;
+
+        const double dt = std::chrono::duration<double>(_info.dt).count();
+        if (dt <= 0.0)
+            return;
+
+        // Pose the model must have at the end of this step
+        const double t =
+            std::chrono::duration<double>(_info.simTime).count() + dt;
+        const gz::math::Pose3d target = this->PoseAt(t);
+        const gz::math::Pose3d current =
+            gz::sim::worldPose(this->model.Entity(), _ecm);
+
+        if (current.Pos().Distance(target.Pos()) > this->teleport_distance)
+        {
+            this->model.SetWorldPoseCmd(_ecm, target);
+            this->SetVelocity(_ecm, gz::math::Vector3d::Zero,
+                              gz::math::Vector3d::Zero);
+            return;
+        }
+
+        // World velocities that take the model from its current pose to the
+        // target in one step. This also corrects any drift (gravity, contacts)
+        gz::math::Vector3d lin_vel = (target.Pos() - current.Pos()) / dt;
+
+        gz::math::Vector3d axis;
+        double angle;
+        (target.Rot() * current.Rot().Inverse()).AxisAngle(axis, angle);
+        if (angle > GZ_PI)
+            angle -= 2 * GZ_PI;
+        gz::math::Vector3d ang_vel = axis * (angle / dt);
+
+        // Physics expects model velocity commands in the model frame
+        this->SetVelocity(_ecm,
+                          current.Rot().RotateVectorReverse(lin_vel),
+                          current.Rot().RotateVectorReverse(ang_vel));
+    }
+
+private:
+    // ============================================
+    // HELPERS
+    // ============================================
+    gz::math::Pose3d PoseAt(double _t) const
+    {
+        const Waypoint &first = this->waypoints.front();
+        const Waypoint &last = this->waypoints.back();
+
+        if (this->loop && last.time > 0.0)
+            _t = std::fmod(_t, last.time);
+
+        if (_t <= first.time)
+            return first.pose;
+        if (_t >= last.time)
+            return last.pose;
+
+        auto next = std::upper_bound(
+            this->waypoints.begin(), this->waypoints.end(), _t,
+            [](double _time, const Waypoint &_wp) { return _time < _wp.time; });
+        auto prev = std::prev(next);
+
+        const double alpha = (_t - prev->time) / (next->time - prev->time);
+
+        return gz::math::Pose3d(
+            prev->pose.Pos() + (next->pose.Pos() - prev->pose.Pos()) * alpha,
+            gz::math::Quaterniond::Slerp(
+                alpha, prev->pose.Rot(), next->pose.Rot(), true));
+    }
+
+    void SetVelocity(gz::sim::EntityComponentManager &_ecm,
+                     const gz::math::Vector3d &_lin,
+                     const gz::math::Vector3d &_ang)
+    {
+        const gz::sim::Entity entity = this->model.Entity();
+
+        if (!_ecm.Component<gz::sim::components::LinearVelocityCmd>(entity))
+            _ecm.CreateComponent(entity,
+                                 gz::sim::components::LinearVelocityCmd(_lin));
+        else
+            _ecm.SetComponentData<gz::sim::components::LinearVelocityCmd>(
+                entity, _lin);
+
+        if (!_ecm.Component<gz::sim::components::AngularVelocityCmd>(entity))
+            _ecm.CreateComponent(entity,
+                                 gz::sim::components::AngularVelocityCmd(_ang));
+        else
+            _ecm.SetComponentData<gz::sim::components::AngularVelocityCmd>(
+                entity, _ang);
+    }
+};
+}
+
+// ============================================
+// PLUGIN REGISTRATION
+// ============================================
+GZ_ADD_PLUGIN(
+    waypoint_follower::WaypointFollower,
+    gz::sim::System,
+    waypoint_follower::WaypointFollower::ISystemConfigure,
+    waypoint_follower::WaypointFollower::ISystemPreUpdate
+)
+
+GZ_ADD_PLUGIN_ALIAS(waypoint_follower::WaypointFollower,
+                    "waypoint_follower::WaypointFollower")

Launchers/visual_lander.launch.py

@@ -1,9 +1,12 @@
 import os
 
-from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import (
+    get_package_prefix,
+    get_package_share_directory,
+)
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import AppendEnvironmentVariable, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -15,6 +18,12 @@ def generate_launch_description():
     worlds_dir = "/opt/jderobot/Scenes"
     world_path = os.path.join(worlds_dir, world_file_name)
 
+    # Make the waypoint_follower_harmonic system plugin (moves the car) discoverable by gz
+    set_gz_plugin_path = AppendEnvironmentVariable(
+        name="GZ_SIM_SYSTEM_PLUGIN_PATH",
+        value=os.path.join(get_package_prefix("custom_robots"), "lib"),
+    )
+
     gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")
@@ -42,6 +51,7 @@ def generate_launch_description():
     )
 
     ld = LaunchDescription()
+    ld.add_action(set_gz_plugin_path)
     ld.add_action(gazebo_server)
     ld.add_action(world_entity_cmd)
     ld.add_action(gz_ros2_bridge)

Launchers/visual_lander_circuit.launch.py

@@ -1,9 +1,12 @@
 import os
 
-from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import (
+    get_package_prefix,
+    get_package_share_directory,
+)
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import AppendEnvironmentVariable, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
 
@@ -15,6 +18,12 @@ def generate_launch_description():
     worlds_dir = "/opt/jderobot/Scenes"
     world_path = os.path.join(worlds_dir, world_file_name)
 
+    # Make the waypoint_follower_harmonic system plugin (moves the car) discoverable by gz
+    set_gz_plugin_path = AppendEnvironmentVariable(
+        name="GZ_SIM_SYSTEM_PLUGIN_PATH",
+        value=os.path.join(get_package_prefix("custom_robots"), "lib"),
+    )
+
     gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(ros_gz_sim, "launch", "gz_sim.launch.py")
@@ -42,6 +51,7 @@ def generate_launch_description():
     )
 
     ld = LaunchDescription()
+    ld.add_action(set_gz_plugin_path)
     ld.add_action(gazebo_server)
     ld.add_action(world_entity_cmd)
     ld.add_action(gz_ros2_bridge)

Scenes/visual_lander.world

@@ -82,23 +82,54 @@
 			<pose>10 1 0.01 0 0 1.5708</pose>
 		</include>
 
-		<actor name="car">
-			<pose>0 0 0.75 0 0 0</pose>
-			<skin>
-				<filename>model://car_color_beacon/meshes/model.dae</filename>
-				<scale>1</scale>
-			</skin>
-			<script>
+		<model name="car">
+			<pose>-12 0 0 0 0 1.5707</pose>
+			<link name="car_link">
+				<pose>0 0 0.75 0 0 0</pose>
+				<inertial>
+					<mass>1200</mass>
+					<inertia>
+						<ixx>3194.0</ixx>
+						<ixy>0</ixy>
+						<ixz>0</ixz>
+						<iyy>981.0</iyy>
+						<iyz>0</iyz>
+						<izz>3837.0</izz>
+					</inertia>
+				</inertial>
+				<!-- Car body, kept above the ground so it does not drag on it -->
+				<collision name="body_collision">
+					<pose>0 0 0.05 0 0 0</pose>
+					<geometry>
+						<box>
+							<size>2.85 5.5 1.0</size>
+						</box>
+					</geometry>
+				</collision>
+				<!-- Roof with the beacon, where the drone lands -->
+				<collision name="roof_collision">
+					<pose>0 0.45 0.92 0 0 0</pose>
+					<geometry>
+						<box>
+							<size>2.0 2.0 0.74</size>
+						</box>
+					</geometry>
+				</collision>
+				<visual name="visual">
+					<geometry>
+						<mesh>
+							<uri>model://car_color_beacon/meshes/model.dae</uri>
+						</mesh>
+					</geometry>
+				</visual>
+			</link>
+			<plugin filename="libwaypoint_follower_harmonic.so" name="waypoint_follower::WaypointFollower">
 				<loop>true</loop>
-				<delay_start>0.0</delay_start>
-				<auto_start>true</auto_start>
-				<trajectory id="0" type="back_and_forth">
-					<waypoint><time>0.0</time><pose>-12 0 0 0 0 1.5707</pose></waypoint>
-					<waypoint><time>34.3</time><pose>12 0 0 0 0 1.5707</pose></waypoint>
-					<waypoint><time>68.6</time><pose>-12 0 0 0 0 1.5707</pose></waypoint>
-				</trajectory>
-			</script>
-		</actor>
+				<waypoint><time>0.0</time><pose>-12 0 0 0 0 1.5707</pose></waypoint>
+				<waypoint><time>34.3</time><pose>12 0 0 0 0 1.5707</pose></waypoint>
+				<waypoint><time>68.6</time><pose>-12 0 0 0 0 1.5707</pose></waypoint>
+			</plugin>
+		</model>
 
 		<include>
 			<uri>model://pine_tree</uri>