Feature/fleet sil

CMakeLists.txt

@@ -2,13 +2,33 @@ cmake_minimum_required(VERSION 3.5)
 project(edu_simulation)
 
 find_package(ament_cmake REQUIRED)
+find_package(gazebo_dev REQUIRED)
+find_package(gazebo_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(edu_robot REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED On)
+set(CMAKE_CXX_EXTENSIONS Off)
+
+add_subdirectory(src)
 
 # install(DIRECTORY world  
 #   DESTINATION share/${PROJECT_NAME}
 # )
 
 ament_package()
 
+install(TARGETS
+    eduard-object-detection-sensor-plugin eduard-model-plugin
+  LIBRARY DESTINATION share/${PROJECT_NAME}/lib
+)
+
 install(DIRECTORY model
   DESTINATION share/${PROJECT_NAME}/
 )

docker/host/Dockerfile

@@ -0,0 +1,49 @@
+FROM ros:humble-ros-base-focal
+
+# using bash
+SHELL ["/bin/bash", "-c"]
+ENV ROS_DISTRO humble
+ENV USER user
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN \
+# install Joystick Driver for ROS2 foxy
+    apt-get update \
+    && apt-get install -y \
+    software-properties-common \
+    ros-$ROS_DISTRO-rmw-fastrtps-cpp \
+    ros-$ROS_DISTRO-fastrtps \
+    ros-$ROS_DISTRO-desktop \
+    ros-$ROS_DISTRO-xacro \
+    ros-$ROS_DISTRO-gazebo-ros-pkgs
+
+ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp
+
+# write source commands to .bashrc -> no need to source afterwards
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc \
+    && echo "export RMW_IMPLEMENTATION=rmw_fastrtps_cpp" >> ~/.bashrc \
+########################################################
+# Add non root user for development
+    && useradd --create-home --shell /bin/bash $USER \
+    && echo "$USER ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers \
+    && /sbin/usermod -aG sudo $USER \
+    && echo "export RMW_IMPLEMENTATION=rmw_fastrtps_cpp" >> /home/$USER/.bashrc \
+    && echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> /home/$USER/.bashrc \
+    && echo "source /home/$USER/eduart_ws/install/setup.bash" >> /home/$USER/.bashrc \
+########################################################
+# Clean up
+    && apt-get clean
+
+COPY ./edu_robot /home/$USER/eduart_ws/src/edu_robot
+COPY ./edu_robot_control /home/$USER/eduart_ws/src/edu_robot_control
+COPY ./edu_simulation /home/$USER/eduart_ws/src/edu_simulation
+
+WORKDIR /home/$USER/eduart_ws
+
+RUN export RMW_IMPLEMENTATION=rmw_fastrtps_cpp \
+    && source /opt/ros/$ROS_DISTRO/setup.bash \
+    && colcon build --symlink-install --executor sequential --event-handlers console_direct+ --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+CMD export RMW_IMPLEMENTATION=rmw_fastrtps_cpp \
+    && source /home/$USER/eduart_ws/install/setup.bash \
+    && ros2 launch edu_simulation eduard.launch.py

docker/host/makefile

@@ -0,0 +1,17 @@
+current_dir = $(shell pwd)
+
+download-sources:
+	git clone --branch develop https://github.com/EduArt-Robotik/edu_robot.git
+	git clone --branch main https://github.com/EduArt-Robotik/edu_robot_control.git
+	git clone --branch develop git@github.com:EduArt-Robotik/edu_simulation.git
+
+build-docker:
+	docker build -t eduart-simulation:0.1.0 .
+
+all: download-sources build-docker
+	echo "build edu_simulation into a docker image"
+
+clean:
+	rm -rf edu_robot
+	rm -rf edu_robot_control
+	rm -rf edu_simulation

include/edu_simulation/eduard_gazebo_bot.hpp

@@ -0,0 +1,28 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/eduard/eduard.hpp>
+
+#include <gazebo/gazebo.hh>
+
+namespace eduart {
+namespace simulation {
+
+class EduardGazeboBot : public robot::eduard::Eduard
+{
+public:
+  EduardGazeboBot(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf, const std::string& ns);
+  ~EduardGazeboBot() override;
+
+  void OnUpdate();
+
+private:
+  gazebo::physics::ModelPtr _parent;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/eduard_hardware_component_factory.hpp

@@ -0,0 +1,22 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/hardware_component_factory.hpp>
+
+#include <gazebo/gazebo.hh>
+
+namespace eduart {
+namespace simulation {
+
+class EduardHardwareComponentFactory : public robot::HardwareComponentFactory
+{
+public:
+  EduardHardwareComponentFactory(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf, rclcpp::Node& ros_node);
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/eduard_model_plugin.hpp

@@ -0,0 +1,35 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <gazebo/gazebo.hh>
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo_ros/executor.hpp>
+
+// #include <
+
+namespace eduart {
+namespace simulation {
+
+class EduardModelPlugin : public gazebo::ModelPlugin
+{
+public:
+  EduardModelPlugin();
+
+  void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) override;
+
+private:
+  void OnUpdate();
+
+  gazebo::physics::ModelPtr _model;
+  gazebo::event::ConnectionPtr _update_connection;
+  gazebo::event::ConnectionPtr _update_bot_connection;  
+  std::shared_ptr<gazebo_ros::Executor> _ros_executer;
+  std::shared_ptr<rclcpp::Node> _robot_ros_node;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/gazebo_hardware_adapter.hpp

@@ -0,0 +1,27 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/robot_hardware_interface.hpp>
+
+#include <gazebo/gazebo.hh>
+
+namespace eduart {
+namespace simulation {
+
+class GazeboHardwareAdapter : public robot::RobotHardwareInterface
+{
+public:
+  GazeboHardwareAdapter(sdf::ElementPtr sdf);
+  ~GazeboHardwareAdapter() override;
+
+  void enable() override;
+  void disable() override;
+  robot::RobotStatusReport getStatusReport() override;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/gazebo_imu_sensor.hpp

@@ -0,0 +1,32 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/hardware_component_interface.hpp>
+#include <edu_robot/imu_sensor.hpp>
+
+#include <gazebo/sensors/SensorTypes.hh>
+
+namespace eduart {
+namespace simulation {
+
+class GazeboImuSensor : public robot::ImuSensor::SensorInterface
+{
+public:
+  GazeboImuSensor(gazebo::sensors::SensorPtr sensor, rclcpp::Node& ros_node);
+  ~GazeboImuSensor() override;
+
+  void initialize(const robot::ImuSensor::Parameter& parameter) override;
+
+private:
+  void getMeasurement();
+
+  gazebo::sensors::ImuSensorPtr _sensor;
+  std::shared_ptr<rclcpp::TimerBase> _timer_get_measurement;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/gazebo_lighting.hpp

@@ -0,0 +1,25 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/hardware_component_interface.hpp>
+#include <edu_robot/lighting.hpp>
+
+namespace eduart {
+namespace simulation {
+
+class GazeboLighting : public robot::Lighting::ComponentInterface
+{
+public:
+  GazeboLighting();
+  ~GazeboLighting() override;
+
+  void processSetValue(const robot::Color& color, const robot::Lighting::Mode& mode) override;
+  void initialize(const robot::Lighting::Parameter& parameter) override;  
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/gazebo_motor_controller.hpp

@@ -0,0 +1,25 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/motor_controller.hpp>
+
+namespace eduart {
+namespace simulation {
+
+class GazeboMotorController : public robot::MotorController::ComponentInterface
+                            , public robot::MotorController::SensorInterface
+{
+public:
+  GazeboMotorController();
+  ~GazeboMotorController() override;
+
+  void processSetValue(const robot::Rpm& rpm) override;
+  void initialize(const robot::MotorController::Parameter& parameter) override;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/gazebo_range_sensor.hpp

@@ -0,0 +1,32 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include <edu_robot/range_sensor.hpp>
+#include <edu_robot/hardware_component_interface.hpp>
+
+#include <gazebo/plugins/RayPlugin.hh>
+
+namespace eduart {
+namespace simulation {
+
+class GazeboRangeSensor : public robot::RangeSensor::SensorInterface
+{
+public:
+  GazeboRangeSensor(gazebo::sensors::SensorPtr sensor, rclcpp::Node& ros_node);
+  ~GazeboRangeSensor() override;
+
+  void initialize(const robot::RangeSensor::Parameter& parameter) override;
+
+private:
+  void getMeasurement();
+
+  gazebo::sensors::RaySensorPtr _sensor;
+  std::shared_ptr<rclcpp::TimerBase> _timer_get_measurement;
+};
+
+} // end namespace simulation
+} // end namespace eduart

include/edu_simulation/object_detection_sensor.hpp

@@ -0,0 +1,46 @@
+/**
+ * Copyright EduArt Robotik GmbH 2023
+ *
+ * Author: Christian Wendt (christian.wendt@eduart-robotik.com)
+ */
+#pragma once
+
+#include "gazebo/sensors/LogicalCameraSensor.hh"
+
+#include <gazebo/common/CommonTypes.hh>
+#include <gazebo/common/Plugin.hh>
+
+#include <memory>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+namespace eduart {
+namespace simulation {
+
+class ObjectDetectionSensor : public gazebo::SensorPlugin
+{
+public:
+  ObjectDetectionSensor();
+  ~ObjectDetectionSensor() override;
+
+  void Init() override;
+  void Load(gazebo::sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) override;
+
+private:
+  void OnUpdate();
+
+  std::shared_ptr<gazebo::sensors::LogicalCameraSensor> _logical_camera;
+  gazebo::event::ConnectionPtr _on_update_connection;
+  std::string _filter_string;
+  std::string _frame_id;
+
+  std::shared_ptr<rclcpp::Node> _ros_node;
+  std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::PoseStamped>> _pub_object_pose;
+};
+
+} // end namespace simulation
+} // end namespace eduart