add targets in the environment

Author: MatCat960

CMakeLists.txt

@@ -41,6 +41,7 @@ add_library(${PROJECT_NAME}_lib
   "src/gmm_visualizer.cpp"
   "src/gmm_test_node.cpp"
   "src/hs_interface.cpp"
+  "src/target_publisher_node.cpp"
 )
 
 
@@ -91,6 +92,9 @@ add_executable(gmm_test src/gmm_test_node.cpp )
 # target_link_libraries(gmm_test ${SFML_libraries})
 ament_target_dependencies(gmm_test ${dependencies})
 
+add_executable(target_publisher_node src/target_publisher_node.cpp )
+ament_target_dependencies(target_publisher_node ${dependencies})
+
 
 # Install python modules
 ament_python_install_package(${PROJECT_NAME})
@@ -102,7 +106,7 @@ install(PROGRAMS
   )
 
 
-install(TARGETS centralized_gmm distributed_gmm gmm_visualizer supervisor_gmm gmm_test hs_interface
+install(TARGETS centralized_gmm distributed_gmm gmm_visualizer supervisor_gmm gmm_test hs_interface target_publisher_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

launch/gmm_online_world.launch.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+import os
+import random
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import ThisLaunchFileDir
+from launch.actions import ExecuteProcess
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL']
+
+AREA_W = 10.0
+ROBOTS_NUM = 8
+TARGETS_NUM = 2
+
+
+def generate_launch_description():
+    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+    launch_file_dir = os.path.join(get_package_share_directory('gazebo_ros'), 'launch')
+    launch_list = []
+
+    gazebo = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([launch_file_dir, '/gazebo.launch.py']),
+            launch_arguments={'gui': 'true'}.items(),
+        )
+
+    launch_list.append(gazebo)
+
+
+    x_pos = []
+    y_pos = []
+    theta = []
+
+    for i in range(ROBOTS_NUM):
+        ranx = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
+        rany = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
+        th = random.uniform(0,6)
+
+        x_pos.append(ranx)
+        y_pos.append(rany)
+        theta.append(th)
+        # print(th)
+
+        
+
+    for i in range(ROBOTS_NUM):
+        spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                            arguments=['-entity', 'turtlebot'+str(i), '-database', 'turtlebot3_' + TURTLEBOT3_MODEL, '-robot_namespace', 'turtlebot'+str(i),
+                                    '-x', str(float(x_pos[i])), '-y', str(float(y_pos[i])), '-z', str(0.1), '-Y', str(theta[i]),
+                            ],
+                            output='screen')
+
+        launch_list.append(spawn_entity)
+
+
+    xtargets = []
+    ytargets = []
+    for i in range(TARGETS_NUM):
+        xt = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
+        yt = random.uniform(-0.5*AREA_W, 0.5*AREA_W)
+        xtargets.append(xt)
+        ytargets.append(yt)
+        target = Node(package='gazebo_ros', executable='spawn_entity.py',
+                            arguments=['-entity', 'target'+str(i), '-database', 'target', '-robot_namespace', 'target'+str(i),
+                                    '-x', str(float(xt)), '-y', str(float(yt)), '-z', str(0.2),
+                            ],
+                            output='screen')
+
+        launch_list.append(target)
+
+    for i in range(TARGETS_NUM):
+        target_pub = Node(
+            package='gmm_coverage',
+            node_executable='target_publisher_node',
+            name='target_pub',
+            parameters=[{"XT": xtargets[i]},
+                        {"YT": ytargets[i]},
+                        {"ID": i}],
+            output='screen')
+    
+        launch_list.append(target_pub)
+        
+
+    return LaunchDescription(launch_list)

src/target_publisher_node.cpp

@@ -0,0 +1,97 @@
+// STL
+#include <iostream>
+#include <vector>
+#include <chrono>
+#include <random>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/impl/utils.h>
+#include <chrono>
+#include <functional>
+#include <memory>
+#include <string>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/SVD>
+#include <cstdlib>
+#include <cmath>
+#include <algorithm>
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <stdlib.h>
+#include <time.h>
+#include <fstream>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/stat.h>
+#include <math.h>
+
+// ROS includes
+#include "rclcpp/rclcpp.hpp"
+#include <geometry_msgs/msg/pose.hpp>
+
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+
+
+class Target : public rclcpp::Node
+{
+
+public:
+    Target() : Node("target_publisher_node")
+    {
+        //------------------------------------------------- ROS parameters ---------------------------------------------------------
+        this->declare_parameter<float>("XT", 0.0);
+        this->get_parameter("XT", XT);
+        this->declare_parameter<float>("YT", 0.0);
+        this->get_parameter("YT", YT);
+        this->declare_parameter<int>("ID", 0);
+        this->get_parameter("ID", ID);
+        
+        //-----------------------------------------------------------------------------------------------------------------------------------
+
+        //--------------------------------------------------- Subscribers and Publishers ----------------------------------------------------
+        pub_ = this->create_publisher<geometry_msgs::msg::Pose>("/target" + std::to_string(ID) + "/pose", 1);
+        timer_ = this->create_wall_timer(100ms, std::bind(&Target::loop, this));
+
+        msg.position.x = XT;
+        msg.position.y = YT;
+        msg.position.z = 0.0;
+
+        std::cout << "Target " << ID << " created in position " << XT << " " << YT << std::endl;
+
+        
+    }
+    ~Target()
+    {
+        std::cout<<"DESTROYER HAS BEEN CALLED"<<std::endl;
+    }
+
+    //void stop(int signum);
+    void loop();
+
+private:
+    double XT;
+    double YT;
+    int ID;
+
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    geometry_msgs::msg::Pose msg;
+};
+
+void Target::loop()
+{
+    pub_->publish(msg);
+}
+
+
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<Target>());
+    rclcpp::shutdown();
+    return 0;
+}