diff --git a/database/exercises/db.sql b/database/exercises/db.sql index e5b43d7f4..86cee7755 100644 --- a/database/exercises/db.sql +++ b/database/exercises/db.sql @@ -124,7 +124,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR 11 montecarlo_laser_loc Montecarlo Laser Loc Calculate the position of the robot based on the ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/montecarlo_laser_loc 12 montecarlo_visual_loc Montecarlo Visual Loc Calculate the position of the robot based on the ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/montecarlo_visual_loc 13 marker_visual_loc Marker Visual Loc Calculate the position of the robot based on the ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/marker_visual_loc -14 laser_mapping Laser Mapping Build a map based on sensor readings ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/laser_mapping +14 laser_mapping Laser Mapping Build a map based on sensor readings ["ROS2", "MULTILANGUAGE"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/MobileRobots/laser_mapping 15 basic_computer_vision Basic Computer Vision Basic Computer Vision exercise using React and RAM ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/basic_computer_vision 16 follow_road Drone Follow Road Drone Follow Road exercise ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/Drones/follow_road 17 pick_place Pick and Place Pick and Place exercise ["ROS2"] PROTOTYPE https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/pick_place diff --git a/exercises/laser_mapping/cpp_lib/CMakeLists.txt b/exercises/laser_mapping/cpp_lib/CMakeLists.txt new file mode 100644 index 000000000..e67a388dc --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.10) +project(ObstacleAvoidanceLibs CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) + +set(ROS2_PACKAGES + rclcpp + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + cv_bridge +) + +foreach(pkg IN LISTS ROS2_PACKAGES) + find_package(${pkg} REQUIRED) +endforeach() + +find_package(Boost REQUIRED COMPONENTS system) +find_package(nlohmann_json REQUIRED) +find_package(OpenCV REQUIRED) + +set(COMMON_INTERFACES_INCLUDE "/home/ws/install/common_interfaces_cpp/include") +set(COMMON_INTERFACES_LIB "/home/ws/install/common_interfaces_cpp/lib/libcommon_interfaces_cpp.so") + +set(WEBGUI_ROS_DEPS rclcpp geometry_msgs std_msgs nav_msgs sensor_msgs cv_bridge) +set(WEBGUI_SYS_DEPS Frequency Boost::system nlohmann_json::nlohmann_json ${OpenCV_LIBS}) +set(HAL_ROS_DEPS rclcpp geometry_msgs sensor_msgs nav_msgs cv_bridge) +set(HAL_SYS_DEPS ${OpenCV_LIBS}) + +# --- libFrequency.so --- +add_library(Frequency SHARED src/Frequency.cpp) +target_include_directories(Frequency PUBLIC include) + +# --- libWebGUI.so --- +add_library(WebGUI SHARED + src/WebGUI.cpp + src/Map.cpp +) +target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS}) +ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS}) +target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS} ${COMMON_INTERFACES_LIB}) + +# --- libHAL.so --- +add_library(HAL SHARED src/HAL.cpp) +target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS}) +ament_target_dependencies(HAL ${HAL_ROS_DEPS}) +target_link_libraries(HAL ${HAL_SYS_DEPS} ${COMMON_INTERFACES_LIB}) \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/include/Frequency.hpp b/exercises/laser_mapping/cpp_lib/include/Frequency.hpp new file mode 100644 index 000000000..583e9723a --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/include/Frequency.hpp @@ -0,0 +1,22 @@ +#ifndef INCLUDE_FREQUENCY_HPP_ +#define INCLUDE_FREQUENCY_HPP_ + +#include +#include +#include + +class Frequency +{ +private: + std::chrono::high_resolution_clock::time_point last_time; + int is_first_iteration; + +public: + Frequency(); + ~Frequency(); + + static int rate; + void tick(int ideal_cycle = 50); +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/include/HAL.hpp b/exercises/laser_mapping/cpp_lib/include/HAL.hpp new file mode 100644 index 000000000..7990979a3 --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/include/HAL.hpp @@ -0,0 +1,37 @@ +#ifndef INCLUDE_HAL_HPP_ +#define INCLUDE_HAL_HPP_ + +#include +#include +#include +#include + +class MotorsNode; +class OdometryNode; +class LaserNode; +namespace rclcpp::executors { class MultiThreadedExecutor; } + +class HAL +{ +public: + HAL() = delete; + + static void set_v(const float velocity); + static void set_w(const float velocity); + static std::array get_pose3d(); + static std::array get_odom(); + static std::vector get_laser_data(); + +private: + static void init(); + friend class SystemBootstrapper; + + static std::shared_ptr motors_node_; + static std::shared_ptr odometry_node_; + static std::shared_ptr noisy_odometry_node_; + static std::shared_ptr laser_node_; + static std::shared_ptr executor_; + static std::thread spin_thread_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/include/Map.hpp b/exercises/laser_mapping/cpp_lib/include/Map.hpp new file mode 100644 index 000000000..5a09f803e --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/include/Map.hpp @@ -0,0 +1,29 @@ +#ifndef INCLUDE_MAP_HPP_ +#define INCLUDE_MAP_HPP_ + +#include +#include +#include +#include "common_interfaces_cpp/hal/odometry.hpp" + +class Map +{ +public: + Map(std::function pose_getter, std::function noisy_pose_getter); + + cv::Mat rtx(double angle, double tx, double ty, double tz); + cv::Mat rty(double angle, double tx, double ty, double tz); + cv::Mat rtz(double angle, double tx, double ty, double tz); + cv::Mat rt_vacuum(); + + std::tuple get_robot_coordinates(); + std::tuple get_robot_coordinates_with_noise(); + + void reset(); + +private: + std::function pose_getter_; + std::function noisy_pose_getter_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/include/WebGUI.hpp b/exercises/laser_mapping/cpp_lib/include/WebGUI.hpp new file mode 100644 index 000000000..664e9a46d --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/include/WebGUI.hpp @@ -0,0 +1,29 @@ +#ifndef INCLUDE_WEBGUI_HPP_ +#define INCLUDE_WEBGUI_HPP_ + +#include +#include +#include +#include + +class WebGUINode; +namespace rclcpp::executors { class MultiThreadedExecutor; } + +class WebGUI +{ +public: + WebGUI() = delete; + + static void set_user_map(const cv::Mat& image); + static std::vector pose_to_map(double x_prime, double y_prime, double yaw_prime); + +private: + static void init(); + friend class SystemBootstrapper; + + static std::shared_ptr gui_node_; + static std::shared_ptr executor_; + static std::thread spin_thread_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/src/Frequency.cpp b/exercises/laser_mapping/cpp_lib/src/Frequency.cpp new file mode 100644 index 000000000..4393db325 --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/src/Frequency.cpp @@ -0,0 +1,34 @@ +#include "Frequency.hpp" + +int Frequency::rate = 0; + +Frequency::Frequency() : is_first_iteration(1) {} + +Frequency::~Frequency() {} + +void Frequency::tick(int ideal_cycle) +{ + const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle); + const auto now = std::chrono::high_resolution_clock::now(); + + if (is_first_iteration == 1) + { + last_time = now; + is_first_iteration = 0; + return; + } + + const auto iter_ms = std::chrono::duration_cast(now - last_time); + + if (iter_ms < ideal_ms) + { + rate = std::round(1000.0 / ideal_ms.count()); + std::this_thread::sleep_for(ideal_ms - iter_ms); + } + else + { + rate = std::round(1000.0 / iter_ms.count()); + } + + last_time = now; +} \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/src/HAL.cpp b/exercises/laser_mapping/cpp_lib/src/HAL.cpp new file mode 100644 index 000000000..e0799d5bf --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/src/HAL.cpp @@ -0,0 +1,75 @@ +#include "HAL.hpp" +#include "common_interfaces_cpp/hal/motors.hpp" +#include "common_interfaces_cpp/hal/odometry.hpp" +#include "common_interfaces_cpp/hal/laser.hpp" +#include "rclcpp/rclcpp.hpp" +#include + +using namespace std::chrono_literals; + +std::shared_ptr HAL::motors_node_ = nullptr; +std::shared_ptr HAL::odometry_node_ = nullptr; +std::shared_ptr HAL::noisy_odometry_node_ = nullptr; +std::shared_ptr HAL::laser_node_ = nullptr; +std::shared_ptr HAL::executor_ = nullptr; +std::thread HAL::spin_thread_; + +void HAL::init() +{ + if (!motors_node_) { + odometry_node_ = std::make_shared("/turtlebot3/odom", "hal_odometry"); + noisy_odometry_node_ = std::make_shared("/turtlebot3/odom_noisy", "hal_noisy_odometry_node"); + motors_node_ = std::make_shared("/turtlebot3/cmd_vel", 4.0, 0.3, "hal_motors"); + laser_node_ = std::make_shared("/turtlebot3/laser/scan", "hal_laser"); + + executor_ = std::make_shared(); + executor_->add_node(odometry_node_); + executor_->add_node(noisy_odometry_node_); + executor_->add_node(motors_node_); + executor_->add_node(laser_node_); + + spin_thread_ = std::thread([]() { + executor_->spin(); + }); + spin_thread_.detach(); + } +} + +void HAL::set_v(const float velocity) +{ + if (motors_node_) motors_node_->sendV(static_cast(velocity)); +} + +void HAL::set_w(const float velocity) +{ + if (motors_node_) motors_node_->sendW(static_cast(velocity)); +} + +std::array HAL::get_pose3d() +{ + if (!odometry_node_) return {0.0, 0.0, 0.0}; + + auto raw_pose = odometry_node_->getPose3d(); + return {raw_pose.x, raw_pose.y, raw_pose.yaw}; +} + +std::array HAL::get_odom() +{ + if (!noisy_odometry_node_) return {0.0, 0.0, 0.0}; + + auto raw_pose = noisy_odometry_node_->getPose3d(); + return {raw_pose.x, raw_pose.y, raw_pose.yaw}; +} + +std::vector HAL::get_laser_data() +{ + if (!laser_node_) return {}; + + auto raw_laser = laser_node_->getLaserData(); + while (raw_laser.values.empty() && rclcpp::ok()) { + std::this_thread::sleep_for(5ms); + raw_laser = laser_node_->getLaserData(); + } + + return raw_laser.values; +} \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/src/Map.cpp b/exercises/laser_mapping/cpp_lib/src/Map.cpp new file mode 100644 index 000000000..618a67fed --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/src/Map.cpp @@ -0,0 +1,63 @@ +#include "Map.hpp" +#include + +Map::Map(std::function pose_getter, std::function noisy_pose_getter) + : pose_getter_(pose_getter), noisy_pose_getter_(noisy_pose_getter) +{ +} + +cv::Mat Map::rtx(double angle, double tx, double ty, double tz) +{ + return (cv::Mat_(4, 4) << + 1, 0, 0, tx, + 0, std::cos(angle), -std::sin(angle), ty, + 0, std::sin(angle), std::cos(angle), tz, + 0, 0, 0, 1); +} + +cv::Mat Map::rty(double angle, double tx, double ty, double tz) +{ + return (cv::Mat_(4, 4) << + std::cos(angle), 0, std::sin(angle), tx, + 0, 1, 0, ty, + -std::sin(angle), 0, std::cos(angle), tz, + 0, 0, 0, 1); +} + +cv::Mat Map::rtz(double angle, double tx, double ty, double tz) +{ + return (cv::Mat_(4, 4) << + std::cos(angle), -std::sin(angle), 0, tx, + std::sin(angle), std::cos(angle), 0, ty, + 0, 0, 1, tz, + 0, 0, 0, 1); +} + +cv::Mat Map::rt_vacuum() +{ + return rtz(CV_PI / 2.0, 50, 70, 0); +} + +std::tuple Map::get_robot_coordinates() +{ + Pose3d pose = pose_getter_(); + + double y = -23.53 * (-31.95 - pose.y); + double x = -23.58 * (-20.36 - pose.x); + + return {x, y, pose.yaw}; +} + +std::tuple Map::get_robot_coordinates_with_noise() +{ + Pose3d pose = noisy_pose_getter_(); + + double y = -23.53 * (-31.95 - pose.y); + double x = -23.58 * (-20.36 - pose.x); + + return {x, y, pose.yaw}; +} + +void Map::reset() +{ +} \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_lib/src/WebGUI.cpp b/exercises/laser_mapping/cpp_lib/src/WebGUI.cpp new file mode 100644 index 000000000..9f9e3ed71 --- /dev/null +++ b/exercises/laser_mapping/cpp_lib/src/WebGUI.cpp @@ -0,0 +1,179 @@ +#include "WebGUI.hpp" +#include "Map.hpp" +#include "common_interfaces_cpp/webgui/WebGUIBridge.hpp" +#include "common_interfaces_cpp/webgui/RTFMonitor.hpp" +#include "common_interfaces_cpp/hal/odometry.hpp" +#include "sensor_msgs/msg/image.hpp" +#include +#include +#include +#include +#include + +class WebGUINode : public BaseWebGUI +{ +public: + WebGUINode() + : BaseWebGUI("webgui_node", "127.0.0.1", "2303", 30.0) + , image_updated_(false) + , last_image_payload_("{\"user_map\":null,\"shape\":0}") + , gui_iterations_(0) + , rtf_monitor_("/stats", std::chrono::milliseconds(500)) + , map_util_([this](){ return odom_node_->getPose3d(); }, + [this](){ return noisy_odom_node_->getPose3d(); }) + { + odom_node_ = std::make_shared("/turtlebot3/odom", "webgui_odom"); + noisy_odom_node_ = std::make_shared("/turtlebot3/odom_noisy", "webgui_noisy_odom"); + aux_node_ = std::make_shared("webgui_aux"); + + auto qos_transient = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(); + user_map_sub_ = aux_node_->create_subscription( + "/webgui/user_map", qos_transient, + [this](const sensor_msgs::msg::Image::SharedPtr msg) { + try { + cv::Mat img = cv_bridge::toCvShare(msg, "mono8")->image; + set_user_map(img); + } catch (...) {} + }); + + last_stat_time_ = std::chrono::steady_clock::now(); + stats_timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&WebGUINode::send_stats, this)); + } + + std::vector get_internal_nodes() { + return { shared_from_this(), odom_node_, noisy_odom_node_, aux_node_ }; + } + + void set_user_map(const cv::Mat& img) { + if (img.empty()) return; + + if (img.rows != 970 || img.cols != 1500) { + throw std::invalid_argument("map passed has the wrong dimensions, it has to be 970 pixels high and 1500 pixels wide"); + } + + cv::Mat processed_image; + if (img.channels() == 1) { + cv::cvtColor(img, processed_image, cv::COLOR_GRAY2BGR); + } else { + img.copyTo(processed_image); + } + + std::lock_guard lk(img_mtx_); + processed_image.copyTo(img_buf_); + image_updated_.store(true); + } + + std::vector pose_to_map(double x_prime, double y_prime, double yaw_prime) { + double y = -23.58 * (-20.36 - x_prime); + double x = -23.53 * (-31.95 - y_prime); + double yaw = yaw_prime - CV_PI / 2.0; + return {std::round(x), std::round(y), yaw}; + } + +protected: + json update_gui() override { + gui_iterations_++; + + json inner; + inner["user_map"] = encode_image(); + + auto real_pos = map_util_.get_robot_coordinates(); + inner["real_pose"] = "(" + std::to_string(std::get<0>(real_pos)) + ", " + + std::to_string(std::get<1>(real_pos)) + ", " + + std::to_string(std::get<2>(real_pos)) + ")"; + + auto noisy_pos = map_util_.get_robot_coordinates_with_noise(); + inner["noisy_pose"] = "(" + std::to_string(std::get<0>(noisy_pos)) + ", " + + std::to_string(std::get<1>(noisy_pos)) + ", " + + std::to_string(std::get<2>(noisy_pos)) + ")"; + + return inner; + } + +private: + void send_stats() { + auto now = std::chrono::steady_clock::now(); + std::chrono::duration elapsed = now - last_stat_time_; + + double current_freq = 0.0; + if (elapsed.count() > 0.0) { + current_freq = gui_iterations_.exchange(0) / elapsed.count(); + } + last_stat_time_ = now; + + json stats; + stats["brain"] = std::round(current_freq * 10.0) / 10.0; + stats["gui"] = 30.0; + stats["rtf"] = rtf_monitor_.get(); + stats["fps"] = -1.0; + stats["lat"] = -1.0; + + send_to_frontend(stats); + } + + std::string encode_image() { + cv::Mat local; + { + std::lock_guard lk(img_mtx_); + if (!image_updated_.load() || img_buf_.empty()) + return last_image_payload_; + + img_buf_.copyTo(local); + image_updated_.store(false); + } + + std::vector buf; + cv::imencode(".jpg", local, buf, { cv::IMWRITE_JPEG_QUALITY, 60 }); + + json p; + p["user_map"] = base64_encode(buf.data(), buf.size()); + p["shape"] = std::vector{ local.rows, local.cols, local.channels() }; + + last_image_payload_ = p.dump(); + return last_image_payload_; + } + + std::shared_ptr odom_node_; + std::shared_ptr noisy_odom_node_; + rclcpp::Node::SharedPtr aux_node_; + rclcpp::Subscription::SharedPtr user_map_sub_; + + cv::Mat img_buf_; + std::mutex img_mtx_; + std::atomic image_updated_; + std::string last_image_payload_; + + Map map_util_; + RTFMonitor rtf_monitor_; + rclcpp::TimerBase::SharedPtr stats_timer_; + std::atomic gui_iterations_; + std::chrono::time_point last_stat_time_; +}; + +std::shared_ptr WebGUI::gui_node_ = nullptr; +std::shared_ptr WebGUI::executor_ = nullptr; +std::thread WebGUI::spin_thread_; + +void WebGUI::init() +{ + if (gui_node_) return; + gui_node_ = std::make_shared(); + executor_ = std::make_shared(); + for (auto& node : gui_node_->get_internal_nodes()) + executor_->add_node(node); + spin_thread_ = std::thread([]() { executor_->spin(); }); + spin_thread_.detach(); +} + +void WebGUI::set_user_map(const cv::Mat& image) +{ + if (gui_node_) gui_node_->set_user_map(image); +} + +std::vector WebGUI::pose_to_map(double x_prime, double y_prime, double yaw_prime) +{ + if (gui_node_) return gui_node_->pose_to_map(x_prime, y_prime, yaw_prime); + return {0.0, 0.0, 0.0}; +} \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/CMakeLists.txt b/exercises/laser_mapping/cpp_template/CMakeLists.txt new file mode 100644 index 000000000..dbbb01b64 --- /dev/null +++ b/exercises/laser_mapping/cpp_template/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(academy) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(gazebo_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(common_interfaces_cpp REQUIRED) + +find_package(nlohmann_json REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) + +set(dependencies + rclcpp + sensor_msgs + nav_msgs + geometry_msgs + std_msgs + gazebo_msgs + message_filters + tf2_ros + tf2_geometry_msgs + common_interfaces_cpp +) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} + ${CMAKE_CURRENT_SOURCE_DIR}/libs/include + ${CMAKE_CURRENT_SOURCE_DIR}/libs/include/vacuum_cleaner_cpp +) + +link_directories(${CMAKE_CURRENT_SOURCE_DIR}/libs) + +add_library(${PROJECT_NAME} SHARED academy.cpp) +ament_target_dependencies(${PROJECT_NAME} ${dependencies}) +target_link_libraries(${PROJECT_NAME} + nlohmann_json::nlohmann_json +) + +add_executable(academyCode main.cpp) +ament_target_dependencies(academyCode ${dependencies}) +target_link_libraries(academyCode + ${PROJECT_NAME} + HAL + WebGUI + Frequency + nlohmann_json::nlohmann_json + Boost::system + Boost::thread +) + +install(TARGETS + academyCode + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/libs/include/Frequency.hpp b/exercises/laser_mapping/cpp_template/libs/include/Frequency.hpp new file mode 100644 index 000000000..583e9723a --- /dev/null +++ b/exercises/laser_mapping/cpp_template/libs/include/Frequency.hpp @@ -0,0 +1,22 @@ +#ifndef INCLUDE_FREQUENCY_HPP_ +#define INCLUDE_FREQUENCY_HPP_ + +#include +#include +#include + +class Frequency +{ +private: + std::chrono::high_resolution_clock::time_point last_time; + int is_first_iteration; + +public: + Frequency(); + ~Frequency(); + + static int rate; + void tick(int ideal_cycle = 50); +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/libs/include/HAL.hpp b/exercises/laser_mapping/cpp_template/libs/include/HAL.hpp new file mode 100644 index 000000000..7990979a3 --- /dev/null +++ b/exercises/laser_mapping/cpp_template/libs/include/HAL.hpp @@ -0,0 +1,37 @@ +#ifndef INCLUDE_HAL_HPP_ +#define INCLUDE_HAL_HPP_ + +#include +#include +#include +#include + +class MotorsNode; +class OdometryNode; +class LaserNode; +namespace rclcpp::executors { class MultiThreadedExecutor; } + +class HAL +{ +public: + HAL() = delete; + + static void set_v(const float velocity); + static void set_w(const float velocity); + static std::array get_pose3d(); + static std::array get_odom(); + static std::vector get_laser_data(); + +private: + static void init(); + friend class SystemBootstrapper; + + static std::shared_ptr motors_node_; + static std::shared_ptr odometry_node_; + static std::shared_ptr noisy_odometry_node_; + static std::shared_ptr laser_node_; + static std::shared_ptr executor_; + static std::thread spin_thread_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/libs/include/Map.hpp b/exercises/laser_mapping/cpp_template/libs/include/Map.hpp new file mode 100644 index 000000000..5a09f803e --- /dev/null +++ b/exercises/laser_mapping/cpp_template/libs/include/Map.hpp @@ -0,0 +1,29 @@ +#ifndef INCLUDE_MAP_HPP_ +#define INCLUDE_MAP_HPP_ + +#include +#include +#include +#include "common_interfaces_cpp/hal/odometry.hpp" + +class Map +{ +public: + Map(std::function pose_getter, std::function noisy_pose_getter); + + cv::Mat rtx(double angle, double tx, double ty, double tz); + cv::Mat rty(double angle, double tx, double ty, double tz); + cv::Mat rtz(double angle, double tx, double ty, double tz); + cv::Mat rt_vacuum(); + + std::tuple get_robot_coordinates(); + std::tuple get_robot_coordinates_with_noise(); + + void reset(); + +private: + std::function pose_getter_; + std::function noisy_pose_getter_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/libs/include/WebGUI.hpp b/exercises/laser_mapping/cpp_template/libs/include/WebGUI.hpp new file mode 100644 index 000000000..664e9a46d --- /dev/null +++ b/exercises/laser_mapping/cpp_template/libs/include/WebGUI.hpp @@ -0,0 +1,29 @@ +#ifndef INCLUDE_WEBGUI_HPP_ +#define INCLUDE_WEBGUI_HPP_ + +#include +#include +#include +#include + +class WebGUINode; +namespace rclcpp::executors { class MultiThreadedExecutor; } + +class WebGUI +{ +public: + WebGUI() = delete; + + static void set_user_map(const cv::Mat& image); + static std::vector pose_to_map(double x_prime, double y_prime, double yaw_prime); + +private: + static void init(); + friend class SystemBootstrapper; + + static std::shared_ptr gui_node_; + static std::shared_ptr executor_; + static std::thread spin_thread_; +}; + +#endif \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/libs/libFrequency.so b/exercises/laser_mapping/cpp_template/libs/libFrequency.so new file mode 100755 index 000000000..3b6d3ca2e Binary files /dev/null and b/exercises/laser_mapping/cpp_template/libs/libFrequency.so differ diff --git a/exercises/laser_mapping/cpp_template/libs/libHAL.so b/exercises/laser_mapping/cpp_template/libs/libHAL.so new file mode 100755 index 000000000..d09895d06 Binary files /dev/null and b/exercises/laser_mapping/cpp_template/libs/libHAL.so differ diff --git a/exercises/laser_mapping/cpp_template/libs/libWebGUI.so b/exercises/laser_mapping/cpp_template/libs/libWebGUI.so new file mode 100755 index 000000000..3d93a9ee8 Binary files /dev/null and b/exercises/laser_mapping/cpp_template/libs/libWebGUI.so differ diff --git a/exercises/laser_mapping/cpp_template/main.cpp b/exercises/laser_mapping/cpp_template/main.cpp new file mode 100644 index 000000000..160569d9a --- /dev/null +++ b/exercises/laser_mapping/cpp_template/main.cpp @@ -0,0 +1,65 @@ +#include "HAL.hpp" +#include "WebGUI.hpp" +#include "academy.cpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include + +class SystemBootstrapper { +public: + static void init_hal() { + HAL::init(); + } + + static void init_webgui() { + WebGUI::init(); + } +}; + +void start_console() +{ + int virtual_terminal = 0; + for (const auto &entry : std::filesystem::directory_iterator("/dev/pts/")) + { + std::filesystem::path outfilename = entry.path(); + std::string filename = outfilename.filename().string(); + if (filename != "ptmx" && std::stoi(filename) > virtual_terminal) + { + virtual_terminal = std::stoi(filename); + } + } + + const std::string v_terminal_str = "/dev/pts/" + std::to_string(virtual_terminal); + + if (freopen(v_terminal_str.c_str(), "w", stdout) == NULL) {} + if (freopen(v_terminal_str.c_str(), "w", stderr) == NULL) {} + if (freopen(v_terminal_str.c_str(), "w", stdin) == NULL) {} +} + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + start_console(); + + SystemBootstrapper::init_webgui(); + +#ifdef USER_NODE + rclcpp::executors::SingleThreadedExecutor executor; + auto user_node = std::make_shared(); + executor.add_node(user_node); + executor.spin(); +#else + SystemBootstrapper::init_hal(); + + std::thread user_thread(exercise); + + if (user_thread.joinable()) { + user_thread.join(); + } +#endif + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/exercises/laser_mapping/cpp_template/package.xml b/exercises/laser_mapping/cpp_template/package.xml new file mode 100644 index 000000000..2358852a7 --- /dev/null +++ b/exercises/laser_mapping/cpp_template/package.xml @@ -0,0 +1,30 @@ + + + + academy + 0.0.0 + Follow line C++ exercise + javier + TODO: License declaration + + ament_cmake + + rclcpp + geometry_msgs + libopencv-dev + sensor_msgs + std_msgs + nav_msgs + message_filters + gazebo_msgs + tf2_ros + tf2_geometry_msgs + nlohmann-json-dev + common_interfaces_cpp + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file