Skip to content

Commit dd85d42

Browse files
aquintan4aquintan4
committed
create a lib with the simple api in c++ for basic vacuum cleaner
1 parent 3e882f9 commit dd85d42

8 files changed

Lines changed: 392 additions & 0 deletions

File tree

‎ra_cpp_api/vacuum_cleaner_cpp/CMakeLists.txt‎

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(vacuum_cleaner_cpp)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
find_package(ament_cmake REQUIRED)
9+
find_package(rclcpp REQUIRED)
10+
find_package(nav_msgs REQUIRED)
11+
find_package(gazebo_msgs REQUIRED)
12+
find_package(tf2 REQUIRED)
13+
find_package(tf2_geometry_msgs REQUIRED)
14+
find_package(common_interfaces_cpp REQUIRED)
15+
find_package(Boost REQUIRED COMPONENTS system)
16+
find_package(nlohmann_json REQUIRED)
17+
18+
add_library(${PROJECT_NAME}_lib SHARED
19+
src/HAL.cpp
20+
src/Frequency.cpp
21+
src/WebGUI.cpp
22+
)
23+
24+
target_include_directories(${PROJECT_NAME}_lib PUBLIC
25+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
26+
$<INSTALL_INTERFACE:include>
27+
)
28+
29+
target_link_libraries(${PROJECT_NAME}_lib
30+
Boost::system
31+
nlohmann_json::nlohmann_json
32+
)
33+
34+
ament_target_dependencies(${PROJECT_NAME}_lib
35+
rclcpp
36+
nav_msgs
37+
gazebo_msgs
38+
tf2
39+
tf2_geometry_msgs
40+
common_interfaces_cpp
41+
)
42+
43+
install(TARGETS ${PROJECT_NAME}_lib
44+
EXPORT export_${PROJECT_NAME}
45+
LIBRARY DESTINATION lib
46+
ARCHIVE DESTINATION lib
47+
RUNTIME DESTINATION bin
48+
)
49+
50+
install(DIRECTORY include/
51+
DESTINATION include
52+
)
53+
54+
ament_export_include_directories(include)
55+
ament_export_libraries(${PROJECT_NAME}_lib)
56+
ament_export_targets(export_${PROJECT_NAME})
57+
ament_export_dependencies(
58+
rclcpp
59+
nav_msgs
60+
gazebo_msgs
61+
tf2
62+
tf2_geometry_msgs
63+
common_interfaces_cpp
64+
Boost
65+
)
66+
67+
ament_package()

‎ra_cpp_api/vacuum_cleaner_cpp/include/vacuum_cleaner_cpp/Frecuency.hpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef VACUUM_CLEANER_CPP__FREQUENCY_HPP_
2+
#define VACUUM_CLEANER_CPP__FREQUENCY_HPP_
3+
4+
#include <chrono>
5+
#include <cmath>
6+
#include <thread>
7+
8+
class Frequency
9+
{
10+
private:
11+
std::chrono::high_resolution_clock::time_point last_time;
12+
int is_first_iteration;
13+
14+
public:
15+
Frequency();
16+
~Frequency();
17+
18+
static int rate;
19+
void tick(int ideal_cycle = 50);
20+
};
21+
22+
#endif

‎ra_cpp_api/vacuum_cleaner_cpp/include/vacuum_cleaner_cpp/HAL.hpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef VACUUM_CLEANER_CPP__HAL_HPP_
2+
#define VACUUM_CLEANER_CPP__HAL_HPP_
3+
4+
#include <vector>
5+
#include <memory>
6+
#include <thread>
7+
#include <chrono>
8+
#include "rclcpp/rclcpp.hpp"
9+
#include "common_interfaces_cpp/hal/motors.hpp"
10+
#include "common_interfaces_cpp/hal/laser.hpp"
11+
#include "common_interfaces_cpp/hal/bumper.hpp"
12+
13+
class HAL : public rclcpp::Node
14+
{
15+
public:
16+
HAL();
17+
static void set_v(const float speed);
18+
static void set_w(const float speed);
19+
static const LaserData *get_laser_data();
20+
static std::vector<bool> get_bumper_data();
21+
22+
private:
23+
static std::shared_ptr<MotorsNode> motors_node_;
24+
static std::shared_ptr<LaserNode> laser_node_;
25+
static std::shared_ptr<BumperNode> bumper_node_;
26+
std::thread spin_thread_;
27+
};
28+
29+
#endif

‎ra_cpp_api/vacuum_cleaner_cpp/include/vacuum_cleaner_cpp/WebGUI.hpp‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
#ifndef VACUUM_CLEANER_CPP__WEBGUI_HPP_
2+
#define VACUUM_CLEANER_CPP__WEBGUI_HPP_
3+
4+
#include <boost/beast/core.hpp>
5+
#include <boost/beast/websocket.hpp>
6+
#include <boost/asio/connect.hpp>
7+
#include <boost/asio/ip/tcp.hpp>
8+
#include <nlohmann/json.hpp>
9+
#include "rclcpp/rclcpp.hpp"
10+
#include "nav_msgs/msg/odometry.hpp"
11+
#include "gazebo_msgs/msg/performance_metrics.hpp"
12+
#include "vacuum_cleaner_cpp/Frequency.hpp"
13+
14+
namespace beast = boost::beast;
15+
namespace websocket = beast::websocket;
16+
namespace net = boost::asio;
17+
using tcp = net::ip::tcp;
18+
using json = nlohmann::json;
19+
20+
class WebGUI
21+
{
22+
public:
23+
WebGUI();
24+
static std::string img_payload;
25+
};
26+
27+
class WebGUINode : public rclcpp::Node
28+
{
29+
public:
30+
WebGUINode();
31+
static std::vector<double> get_pose();
32+
static double get_performance();
33+
34+
private:
35+
void pose_callback(nav_msgs::msg::Odometry::UniquePtr msg);
36+
void performance_callback(gazebo_msgs::msg::PerformanceMetrics::UniquePtr msg);
37+
38+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
39+
rclcpp::Subscription<gazebo_msgs::msg::PerformanceMetrics>::SharedPtr perf_sub_;
40+
41+
static nav_msgs::msg::Odometry last_odom;
42+
static gazebo_msgs::msg::PerformanceMetrics last_perf;
43+
};
44+
45+
#endif

‎ra_cpp_api/vacuum_cleaner_cpp/package.xml‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>vacuum_cleaner_cpp</name>
5+
<version>0.0.1</version>
6+
<description>Vacuum cleaner simulation HAL and WebGUI library</description>
7+
<maintainer email="user@todo.todo">user</maintainer>
8+
<license>Apache License 2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>nav_msgs</depend>
14+
<depend>gazebo_msgs</depend>
15+
<depend>tf2</depend>
16+
<depend>tf2_geometry_msgs</depend>
17+
<depend>common_interfaces_cpp</depend>
18+
<depend>boost</depend>
19+
<depend>nlohmann_json_schema_validator_vendor</depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>ament_lint_common</test_depend>
23+
24+
<export>
25+
<build_type>ament_cmake</build_type>
26+
</export>
27+
</package>

‎ra_cpp_api/vacuum_cleaner_cpp/src/Frequency.cpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#include "vacuum_cleaner_cpp/Frequency.hpp"
2+
3+
int Frequency::rate = 0;
4+
5+
Frequency::Frequency() : is_first_iteration(1) {}
6+
7+
Frequency::~Frequency() {}
8+
9+
void Frequency::tick(int ideal_cycle)
10+
{
11+
const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
12+
const auto now = std::chrono::high_resolution_clock::now();
13+
14+
if (is_first_iteration == 1)
15+
{
16+
last_time = now;
17+
is_first_iteration = 0;
18+
return;
19+
}
20+
21+
const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
22+
23+
if (iter_ms < ideal_ms)
24+
{
25+
rate = std::round(1000.0 / ideal_ms.count());
26+
std::this_thread::sleep_for(ideal_ms - iter_ms);
27+
}
28+
else
29+
{
30+
rate = std::round(1000.0 / iter_ms.count());
31+
}
32+
33+
last_time = now;
34+
}

‎ra_cpp_api/vacuum_cleaner_cpp/src/HAL.cpp‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
#include "vacuum_cleaner_cpp/HAL.hpp"
2+
3+
using namespace std::chrono_literals;
4+
5+
std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
6+
std::shared_ptr<LaserNode> HAL::laser_node_ = nullptr;
7+
std::shared_ptr<BumperNode> HAL::bumper_node_ = nullptr;
8+
9+
HAL::HAL() : Node("hal_node")
10+
{
11+
std::vector<std::string> bumper_topics = {
12+
"/roombaROS/events/right_bumper",
13+
"/roombaROS/events/center_bumper",
14+
"/roombaROS/events/left_bumper"
15+
};
16+
17+
motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 1.0, 1.0);
18+
laser_node_ = std::make_shared<LaserNode>("/roombaROS/laser/scan");
19+
bumper_node_ = std::make_shared<BumperNode>(bumper_topics);
20+
21+
spin_thread_ = std::thread([]() {
22+
rclcpp::executors::SingleThreadedExecutor executor;
23+
executor.add_node(HAL::motors_node_);
24+
executor.add_node(HAL::laser_node_);
25+
executor.add_node(HAL::bumper_node_);
26+
27+
while (rclcpp::ok()) {
28+
executor.spin_some();
29+
std::this_thread::sleep_for(std::chrono::milliseconds(10));
30+
}
31+
});
32+
spin_thread_.detach();
33+
}
34+
35+
void HAL::set_v(const float speed)
36+
{
37+
if (motors_node_) motors_node_->sendV(static_cast<double>(speed));
38+
}
39+
40+
void HAL::set_w(const float speed)
41+
{
42+
if (motors_node_) motors_node_->sendW(static_cast<double>(speed));
43+
}
44+
45+
const LaserData *HAL::get_laser_data()
46+
{
47+
if (!laser_node_) return nullptr;
48+
return new LaserData(laser_node_->getLaserData());
49+
}
50+
51+
std::vector<bool> HAL::get_bumper_data()
52+
{
53+
std::vector<bool> v = {false, false, false};
54+
if (bumper_node_) {
55+
BumperData b_data = bumper_node_->getBumperData();
56+
if (b_data.state == 1 && b_data.bumper >= 0 && b_data.bumper < 3) {
57+
v[b_data.bumper] = true;
58+
}
59+
}
60+
return v;
61+
}

0 commit comments

Comments
 (0)