From 6242ee2160bcde153526ad3c62b03d821155b35e Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 09:51:12 +0100 Subject: [PATCH 01/30] migrate CMakeLists and packagexml --- CMakeLists.txt | 31 +++++++++++++++---------------- package.xml | 10 +++++++++- 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dad3b71..7e50b06 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,17 +1,18 @@ cmake_minimum_required(VERSION 3.0.2) project(emc_system) +set(CMAKE_CXX_STANDARD 14) + add_compile_options(-Wall -Werror=all) add_compile_options(-Wextra -Werror=extra) -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - sensor_msgs - nav_msgs - roscpp - tf2 - tf2_ros -) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(roscpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) # find_package(Boost REQUIRED COMPONENTS system program_options) # find_package(PCL REQUIRED) @@ -21,11 +22,8 @@ find_package(OpenCV REQUIRED) # CATKIN EXPORT # ------------------------------------------------------------------------------------------------ -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES bla -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +ament_package( + ) # ------------------------------------------------------------------------------------------------ @@ -53,7 +51,7 @@ add_library(emc_system src/io.cpp src/rate.cpp ) -target_link_libraries(emc_system ${catkin_LIBRARIES}) +ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros) # ------------------------------------------------------------------------------------------------ # TOOLS @@ -86,8 +84,9 @@ target_link_libraries(emc_example2 emc_system) # UNIT TESTS # ------------------------------------------------------------------------------------------------ -if(CATKIN_ENABLE_TESTING) +if(BUILD_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(tests_io test/io.test test/test_io.cpp) - target_link_libraries(tests_io emc_system ${catkin_LIBRARIES}) + target_link_libraries(tests_io emc_system) + ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros) endif() diff --git a/package.xml b/package.xml index a626341..6e12dea 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,4 @@ - + @@ -12,6 +12,7 @@ TODO catkin + rosidl_default_generators geometry_msgs nav_msgs @@ -20,10 +21,17 @@ tf2 tf2_ros + rosidl_default_runtime + doxygen + ament_cmake_gtest + + + ament_cmake + From 35d70a91ca5682790d5460f13cf71892ab023c7d Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 10:09:58 +0100 Subject: [PATCH 02/30] fix some cmake errors --- CMakeLists.txt | 2 +- package.xml | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e50b06..3bebc0f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(emc_system) set(CMAKE_CXX_STANDARD 14) diff --git a/package.xml b/package.xml index 6e12dea..9c4390a 100644 --- a/package.xml +++ b/package.xml @@ -29,8 +29,6 @@ - - ament_cmake From af6dbbf9b3cbbe43f729c5fc27eaa256ac779197 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 11:15:56 +0100 Subject: [PATCH 03/30] fix amend_package placement and remove test --- CMakeLists.txt | 22 ++++++++-------------- 1 file changed, 8 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bebc0f..cd717ad 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,14 +18,6 @@ find_package(tf2_ros REQUIRED) # find_package(PCL REQUIRED) find_package(OpenCV REQUIRED) -# ------------------------------------------------------------------------------------------------ -# CATKIN EXPORT -# ------------------------------------------------------------------------------------------------ - -ament_package( - -) - # ------------------------------------------------------------------------------------------------ # BUILD # ------------------------------------------------------------------------------------------------ @@ -84,9 +76,11 @@ target_link_libraries(emc_example2 emc_system) # UNIT TESTS # ------------------------------------------------------------------------------------------------ -if(BUILD_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(tests_io test/io.test test/test_io.cpp) - target_link_libraries(tests_io emc_system) - ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros) -endif() +#if(BUILD_TESTING) +# find_package(rostest REQUIRED) +# add_rostest_gtest(tests_io test/io.test test/test_io.cpp) +# target_link_libraries(tests_io emc_system) +# ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros rostest) +#endif() + +ament_package() From 9e950e38d7fbf47bbea4d4ee2196183d2d3d167a Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 11:16:16 +0100 Subject: [PATCH 04/30] roscpp to rclcpp --- CMakeLists.txt | 4 ++-- package.xml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cd717ad..462bb0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,7 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) -find_package(roscpp REQUIRED) +find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) @@ -43,7 +43,7 @@ add_library(emc_system src/io.cpp src/rate.cpp ) -ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros) +ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs rclcpp tf2 tf2_ros) # ------------------------------------------------------------------------------------------------ # TOOLS diff --git a/package.xml b/package.xml index 9c4390a..770d114 100644 --- a/package.xml +++ b/package.xml @@ -16,7 +16,7 @@ geometry_msgs nav_msgs - roscpp + rclcpp sensor_msgs tf2 tf2_ros From 8480d3c77dac68463ae85868760f21baf45fcf16 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 16:34:12 +0100 Subject: [PATCH 05/30] migration --- CMakeLists.txt | 6 ++-- include/emc/communication.h | 69 ++++++++++++++++--------------------- include/emc/data.h | 3 +- include/emc/rate.h | 19 +++++----- package.xml | 1 - src/communication.cpp | 43 ++++++++++++++--------- src/io.cpp | 12 +++---- src/rate.cpp | 21 +++++------ 8 files changed, 87 insertions(+), 87 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 462bb0d..4acc97d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ include_directories( ) add_library(emc_system - include/emc/engine.h + #include/emc/engine.h include/emc/communication.h include/emc/data.h include/emc/odom.h @@ -37,9 +37,9 @@ add_library(emc_system include/emc/rate.h include/emc/bumper.h - src/engine.cpp + #src/engine.cpp src/communication.cpp - src/data.cpp + #src/data.cpp src/io.cpp src/rate.cpp ) diff --git a/include/emc/communication.h b/include/emc/communication.h index b768ccf..3eee0c0 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -5,24 +5,26 @@ #include "emc/odom.h" #include "emc/bumper.h" -#include -#include -#include +#include "rclcpp/rclcpp.hpp" +//#include #include -#include -#include +#include "nav_msgs/msg/map_meta_data.hpp" + +#include +#include +#include +#include +#include +#include +#include -#include -#include -#include -#include #include #include namespace emc { -class Communication +class Communication : public rclcpp::Node { public: @@ -44,7 +46,7 @@ class Communication void sendOpendoorRequest(); - void sendMarker(visualization_msgs::Marker marker); + void sendMarker(visualization_msgs::msg::Marker marker); void speak(const std::string& text); @@ -56,55 +58,44 @@ class Communication private: // Base velocity reference + rclcpp::Publisher::SharedPtr pub_base_ref_; + rclcpp::Publisher::SharedPtr pub_open_door_; + rclcpp::Publisher::SharedPtr pub_speak_; + rclcpp::Publisher::SharedPtr pub_play_; + rclcpp::Publisher::SharedPtr pub_marker_; - ros::Publisher pub_base_ref_; - - ros::Publisher pub_open_door_; - - ros::Publisher pub_speak_; - - ros::Publisher pub_play_; - - ros::Publisher pub_marker_; // Position data std::unique_ptr pub_tf2; //has to be defined after ros::init(), which is called in the constructor // Laser data - - ros::CallbackQueue laser_cb_queue_; + rclcpp::Subscription::SharedPtr sub_laser_; ros::Subscriber sub_laser_; - sensor_msgs::LaserScanConstPtr laser_msg_; + sensor_msgs::msg::LaserScanConstPtr laser_msg_; - void laserCallback(const sensor_msgs::LaserScanConstPtr& msg); + void laserCallback(const sensor_msgs::msg::LaserScanConstPtr& msg); // Odometry data - ros::CallbackQueue odom_cb_queue_; - - ros::Subscriber sub_odom_; + rclcpp::Subscription::SharedPtr sub_odom_; - nav_msgs::OdometryConstPtr odom_msg_; + nav_msgs::msg::OdometryConstPtr odom_msg_; - void odomCallback(const nav_msgs::OdometryConstPtr& msg); + void odomCallback(const nav_msgs::msg::OdometryConstPtr& msg); // Bumper data + rclcpp::Subscription::SharedPtr sub_bumper_f_; + rclcpp::Subscription::SharedPtr sub_bumper_b_; - ros::CallbackQueue bumper_f_cb_queue_; - ros::CallbackQueue bumper_b_cb_queue_; - - ros::Subscriber sub_bumper_f_; - ros::Subscriber sub_bumper_b_; - - std_msgs::BoolConstPtr bumper_f_msg_; - std_msgs::BoolConstPtr bumper_b_msg_; + std_msgs::msg::BoolConstPtr bumper_f_msg_; + std_msgs::msg::BoolConstPtr bumper_b_msg_; - void bumperfCallback(const std_msgs::BoolConstPtr& msg); - void bumperbCallback(const std_msgs::BoolConstPtr& msg); + void bumperfCallback(const std_msgs::msg::BoolConstPtr& msg); + void bumperbCallback(const std_msgs::msg::BoolConstPtr& msg); // pose publishing std::string robot_frame_name; diff --git a/include/emc/data.h b/include/emc/data.h index 75e6bda..44afe30 100644 --- a/include/emc/data.h +++ b/include/emc/data.h @@ -32,6 +32,7 @@ struct ControlEffort // ---------------------------------------------------------------------------------------------------- +/* class FSMInterface { @@ -51,7 +52,7 @@ class FSMInterface std::string event_; }; - +*/ } // end namespace emc #endif diff --git a/include/emc/rate.h b/include/emc/rate.h index a36ec32..1484f35 100644 --- a/include/emc/rate.h +++ b/include/emc/rate.h @@ -1,29 +1,32 @@ #ifndef EMC_SYSTEM_RATE_H_ #define EMC_SYSTEM_RATE_H_ -namespace ros +#include "rclcpp/rclcpp.hpp" +#include + +/*namespace rclcpp { -class Rate; +class GenericRate; } - +*/ namespace emc { -class Rate +class Rate2 { public: - Rate(double freq); + Rate2(double freq); - ~Rate(); + ~Rate2(); void sleep(); private: - ros::Rate* rate_; - + rclcpp::GenericRate* rate_; + //rclcpp::Logger logger_; }; diff --git a/package.xml b/package.xml index 770d114..9a847c5 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,6 @@ TODO - catkin rosidl_default_generators geometry_msgs diff --git a/src/communication.cpp b/src/communication.cpp index 5d39dc8..934f91e 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -1,13 +1,10 @@ #include "emc/communication.h" -#include -#include +//#include +//#include #include #include -#include -#include -#include #include namespace emc @@ -34,6 +31,18 @@ Communication::Communication(std::string /*robot_name*/) if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; + sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, _1)); + sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, _1)); + sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, _1)); + sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, _1)); + + pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); + pub_open_door_ = this->create_publisher(open_door_param, 10); + pub_speak_ = this->create_publisher(speak_param, 10); + pub_play_ = this->create_publisher(play_param, 10); + pub_marker = this->create_publisher("/marker", 10); + +/* ros::SubscribeOptions laser_sub_options = ros::SubscribeOptions::create(laser_param, 1, boost::bind(&Communication::laserCallback, this, _1), ros::VoidPtr(), &laser_cb_queue_); sub_laser_ = nh.subscribe(laser_sub_options); @@ -54,7 +63,7 @@ Communication::Communication(std::string /*robot_name*/) pub_play_ = nh.advertise(play_param, 1); pub_marker_ = nh.advertise("/marker", 1); - +*/ pub_tf2 = std::unique_ptr(new tf2_ros::TransformBroadcaster); } @@ -148,7 +157,7 @@ bool Communication::readBackBumperData(BumperData& bumper) void Communication::sendBaseVelocity(double vx, double vy, double va) { - geometry_msgs::Twist ref; + geometry_msgs::msg::Twist ref; ref.linear.x = vx; ref.linear.y = vy; ref.angular.z = va; @@ -160,13 +169,13 @@ void Communication::sendBaseVelocity(double vx, double vy, double va) void Communication::sendOpendoorRequest() { - std_msgs::Empty msg; + std_msgs::msg::Empty msg; pub_open_door_.publish(msg); } // ---------------------------------------------------------------------------------------------------- -void Communication::sendMarker(visualization_msgs::Marker marker) +void Communication::sendMarker(visualization_msgs::msg::Marker marker) { pub_marker_.publish(marker); } @@ -175,22 +184,22 @@ void Communication::sendMarker(visualization_msgs::Marker marker) void Communication::speak(const std::string& text) { - std_msgs::String str; + std_msgs::msg::String str; str.data = text; pub_speak_.publish(str); } void Communication::play(const std::string& file) { - std_msgs::String str; + std_msgs::msg::String str; str.data = file; pub_play_.publish(str); } -void Communication::sendPoseEstimate(const geometry_msgs::Transform& pose) +void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) { // Publish tf transform - geometry_msgs::TransformStamped transformStamped; + geometry_msgs::msg::TransformStamped transformStamped; transformStamped.header.stamp = ros::Time::now(); transformStamped.header.frame_id = "map"; transformStamped.child_frame_id = robot_frame_name; @@ -200,28 +209,28 @@ void Communication::sendPoseEstimate(const geometry_msgs::Transform& pose) // ---------------------------------------------------------------------------------------------------- -void Communication::laserCallback(const sensor_msgs::LaserScanConstPtr& msg) +void Communication::laserCallback(const sensor_msgs::msg::LaserScanConstPtr& msg) { laser_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::odomCallback(const nav_msgs::OdometryConstPtr& msg) +void Communication::odomCallback(const nav_msgs::msg::OdometryConstPtr& msg) { odom_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::bumperfCallback(const std_msgs::BoolConstPtr& msg) +void Communication::bumperfCallback(const std_msgs::msg::BoolConstPtr& msg) { bumper_f_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::bumperbCallback(const std_msgs::BoolConstPtr& msg) +void Communication::bumperbCallback(const std_msgs::msg::BoolConstPtr& msg) { bumper_b_msg_ = msg; } diff --git a/src/io.cpp b/src/io.cpp index 9f843ae..76e9b71 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -2,13 +2,13 @@ #include "emc/communication.h" -#include // for ros::ok() +//#include // for ros::ok() #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -83,7 +83,7 @@ void IO::play(const std::string& file) bool IO::ok() { - return ros::ok(); + return rclcpp::ok(); } bool IO::sendPath(std::vector> path, std::array color, double width, int id) diff --git a/src/rate.cpp b/src/rate.cpp index a3a800c..e361349 100644 --- a/src/rate.cpp +++ b/src/rate.cpp @@ -1,29 +1,26 @@ #include "emc/rate.h" -#include -#include -namespace emc +emc::Rate2::Rate2(double freq) { - -Rate::Rate(double freq) -{ - ros::Time::init(); - rate_ = new ros::Rate(freq); + //rclcpp::Time::init(); + rate_ = new rclcpp::Rate(freq); + //logger_ = rclcpp::get_logger("ratelogger"); } -Rate::~Rate() +emc::Rate2::~Rate2() { if (rate_) + { delete rate_; + } } -void Rate::sleep() +void emc::Rate2::sleep() { if (!rate_->sleep()) { - ROS_WARN_STREAM("Could not complete the cycle in " << rate_->expectedCycleTime() << ", instead took " << rate_->cycleTime()); + RCLCPP_WARN_STREAM(rclcpp::get_logger("ratelogger"), "Could not complete the cycle in "); //<< rate_->period() << ", instead took ??"); } } -} From e8eaf9ceaca176d506fac611065515cac9741ead Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 27 Feb 2024 16:50:01 +0100 Subject: [PATCH 06/30] msg conversion --- include/emc/communication.h | 34 ++++++++++++++++------------------ src/communication.cpp | 26 +++++++++++++------------- 2 files changed, 29 insertions(+), 31 deletions(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index 3eee0c0..7f10fc9 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -14,9 +14,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -53,16 +53,16 @@ class Communication : public rclcpp::Node void play(const std::string& file); // Postion data - void sendPoseEstimate(const geometry_msgs::Transform& pose); + void sendPoseEstimate(const geometry_msgs::msg::Transform& pose); private: // Base velocity reference rclcpp::Publisher::SharedPtr pub_base_ref_; rclcpp::Publisher::SharedPtr pub_open_door_; - rclcpp::Publisher::SharedPtr pub_speak_; - rclcpp::Publisher::SharedPtr pub_play_; - rclcpp::Publisher::SharedPtr pub_marker_; + rclcpp::Publisher::SharedPtr pub_speak_; + rclcpp::Publisher::SharedPtr pub_play_; + rclcpp::Publisher::SharedPtr pub_marker_; // Position data @@ -70,32 +70,30 @@ class Communication : public rclcpp::Node // Laser data - rclcpp::Subscription::SharedPtr sub_laser_; + rclcpp::Subscription::SharedPtr sub_laser_; - ros::Subscriber sub_laser_; + sensor_msgs::msg::LaserScan::SharedPtr laser_msg_; - sensor_msgs::msg::LaserScanConstPtr laser_msg_; - - void laserCallback(const sensor_msgs::msg::LaserScanConstPtr& msg); + void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg); // Odometry data rclcpp::Subscription::SharedPtr sub_odom_; - nav_msgs::msg::OdometryConstPtr odom_msg_; + nav_msgs::msg::Odometry::SharedPtr odom_msg_; - void odomCallback(const nav_msgs::msg::OdometryConstPtr& msg); + void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); // Bumper data rclcpp::Subscription::SharedPtr sub_bumper_f_; rclcpp::Subscription::SharedPtr sub_bumper_b_; - std_msgs::msg::BoolConstPtr bumper_f_msg_; - std_msgs::msg::BoolConstPtr bumper_b_msg_; + std_msgs::msg::Bool::SharedPtr bumper_f_msg_; + std_msgs::msg::Bool::SharedPtr bumper_b_msg_; - void bumperfCallback(const std_msgs::msg::BoolConstPtr& msg); - void bumperbCallback(const std_msgs::msg::BoolConstPtr& msg); + void bumperfCallback(const std_msgs::msg::Bool::SharedPtr msg); + void bumperbCallback(const std_msgs::msg::Bool::SharedPtr msg); // pose publishing std::string robot_frame_name; diff --git a/src/communication.cpp b/src/communication.cpp index 934f91e..7ef3ff4 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -31,16 +31,16 @@ Communication::Communication(std::string /*robot_name*/) if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; - sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, _1)); - sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, _1)); - sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, _1)); - sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, _1)); + sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, _1)); + sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, _1)); + sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, _1)); + sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, _1)); - pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); - pub_open_door_ = this->create_publisher(open_door_param, 10); - pub_speak_ = this->create_publisher(speak_param, 10); - pub_play_ = this->create_publisher(play_param, 10); - pub_marker = this->create_publisher("/marker", 10); + pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); + pub_open_door_ = this->create_publisher(open_door_param, 10); + pub_speak_ = this->create_publisher(speak_param, 10); + pub_play_ = this->create_publisher(play_param, 10); + pub_marker = this->create_publisher("/marker", 10); /* ros::SubscribeOptions laser_sub_options = ros::SubscribeOptions::create(laser_param, 1, boost::bind(&Communication::laserCallback, this, _1), ros::VoidPtr(), &laser_cb_queue_); @@ -209,28 +209,28 @@ void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) // ---------------------------------------------------------------------------------------------------- -void Communication::laserCallback(const sensor_msgs::msg::LaserScanConstPtr& msg) +void Communication::laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { laser_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::odomCallback(const nav_msgs::msg::OdometryConstPtr& msg) +void Communication::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { odom_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::bumperfCallback(const std_msgs::msg::BoolConstPtr& msg) +void Communication::bumperfCallback(const std_msgs::msg::Bool::SharedPtr msg) { bumper_f_msg_ = msg; } // ---------------------------------------------------------------------------------------------------- -void Communication::bumperbCallback(const std_msgs::msg::BoolConstPtr& msg) +void Communication::bumperbCallback(const std_msgs::msg::Bool::SharedPtr msg) { bumper_b_msg_ = msg; } From 64efc410caa75cff30fdfa4eca6a56ae9b66862b Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 10:30:01 +0100 Subject: [PATCH 07/30] comment rosparam functionality and fix typos --- include/emc/communication.h | 2 +- src/communication.cpp | 41 +++++++++++++++++++++++-------------- src/io.cpp | 8 ++++---- 3 files changed, 31 insertions(+), 20 deletions(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index 7f10fc9..83bab8d 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -58,7 +58,7 @@ class Communication : public rclcpp::Node private: // Base velocity reference - rclcpp::Publisher::SharedPtr pub_base_ref_; + rclcpp::Publisher::SharedPtr pub_cmd_vel_; rclcpp::Publisher::SharedPtr pub_open_door_; rclcpp::Publisher::SharedPtr pub_speak_; rclcpp::Publisher::SharedPtr pub_play_; diff --git a/src/communication.cpp b/src/communication.cpp index 7ef3ff4..44eeef6 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -12,15 +12,26 @@ namespace emc // ---------------------------------------------------------------------------------------------------- -Communication::Communication(std::string /*robot_name*/) +Communication::Communication(std::string /*robot_name*/) : rclcpp::Node("emc_system") { - +/* ros::VP_string args; ros::init(args, "emc_system", ros::init_options::AnonymousName); ros::Time::init(); - - ros::NodeHandle nh; +*/ + // ros::NodeHandle nh; std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; + // temp hardcode param names + laser_param = "laser_scan"; + odom_param = "odom"; + bumper_f_param = "bumper_f"; + bumper_b_param = "bumper_b"; + base_ref_param = "cmd_vel"; + open_door_param = "open_door"; + speak_param = "speak"; + play_param = "play"; + /* + // get robot parameters if (!nh.getParam("laser_", laser_param)) {ROS_ERROR_STREAM("Parameter " << "laser_" << " not set");}; if (!nh.getParam("odom_", odom_param)) {ROS_ERROR_STREAM("Parameter " << "odom_" << " not set");}; if (!nh.getParam("bumper_f_", bumper_f_param)) {ROS_ERROR_STREAM("Parameter " << "bumper_f_" << " not set");}; @@ -30,7 +41,7 @@ Communication::Communication(std::string /*robot_name*/) if (!nh.getParam("speak_", speak_param)) {ROS_ERROR_STREAM("Parameter " << "speak_" << " not set");}; if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; - +*/ sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, _1)); sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, _1)); sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, _1)); @@ -40,7 +51,7 @@ Communication::Communication(std::string /*robot_name*/) pub_open_door_ = this->create_publisher(open_door_param, 10); pub_speak_ = this->create_publisher(speak_param, 10); pub_play_ = this->create_publisher(play_param, 10); - pub_marker = this->create_publisher("/marker", 10); + pub_marker_ = this->create_publisher("/marker", 10); /* ros::SubscribeOptions laser_sub_options = ros::SubscribeOptions::create(laser_param, 1, boost::bind(&Communication::laserCallback, this, _1), ros::VoidPtr(), &laser_cb_queue_); @@ -85,7 +96,7 @@ void Communication::init() bool Communication::readLaserData(LaserData& scan) { laser_msg_.reset(); - laser_cb_queue_.callAvailable(); + //laser_cb_queue_.callAvailable(); if (!laser_msg_) return false; @@ -106,7 +117,7 @@ bool Communication::readLaserData(LaserData& scan) bool Communication::readOdometryData(OdometryData& odom) { odom_msg_.reset(); - odom_cb_queue_.callAvailable(); + //odom_cb_queue_.callAvailable(); if (!odom_msg_) return false; @@ -129,7 +140,7 @@ bool Communication::readFrontBumperData(BumperData& bumper) { bumper_f_msg_.reset(); - bumper_f_cb_queue_.callAvailable(); + //bumper_f_cb_queue_.callAvailable(); if (!bumper_f_msg_) return false; @@ -144,7 +155,7 @@ bool Communication::readBackBumperData(BumperData& bumper) { bumper_b_msg_.reset(); - bumper_b_cb_queue_.callAvailable(); + //bumper_b_cb_queue_.callAvailable(); if (!bumper_b_msg_) return false; @@ -162,7 +173,7 @@ void Communication::sendBaseVelocity(double vx, double vy, double va) ref.linear.y = vy; ref.angular.z = va; - pub_base_ref_.publish(ref); + pub_cmd_vel_->publish(ref); } // ---------------------------------------------------------------------------------------------------- @@ -170,14 +181,14 @@ void Communication::sendBaseVelocity(double vx, double vy, double va) void Communication::sendOpendoorRequest() { std_msgs::msg::Empty msg; - pub_open_door_.publish(msg); + pub_open_door_->publish(msg); } // ---------------------------------------------------------------------------------------------------- void Communication::sendMarker(visualization_msgs::msg::Marker marker) { - pub_marker_.publish(marker); + pub_marker_->publish(marker); } // ---------------------------------------------------------------------------------------------------- @@ -186,14 +197,14 @@ void Communication::speak(const std::string& text) { std_msgs::msg::String str; str.data = text; - pub_speak_.publish(str); + pub_speak_->publish(str); } void Communication::play(const std::string& file) { std_msgs::msg::String str; str.data = file; - pub_play_.publish(str); + pub_play_->publish(str); } void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) diff --git a/src/io.cpp b/src/io.cpp index 76e9b71..72abde6 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -88,13 +88,13 @@ bool IO::ok() bool IO::sendPath(std::vector> path, std::array color, double width, int id) { - visualization_msgs::Marker pathMarker; + visualization_msgs::msg::Marker pathMarker; pathMarker.header.frame_id = "map"; pathMarker.header.stamp = ros::Time(); pathMarker.ns = "path"; pathMarker.id = id; - pathMarker.type = visualization_msgs::Marker::LINE_STRIP; - pathMarker.action = visualization_msgs::Marker::ADD; + pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; + pathMarker.action = visualization_msgs::msg::Marker::ADD; pathMarker.color.a = 1.0; pathMarker.color.r = color[0]; pathMarker.color.g = color[1]; @@ -103,7 +103,7 @@ bool IO::sendPath(std::vector> path, std::array c pathMarker.scale.x = width; for (std::vector>::iterator it = path.begin(); it != path.end(); ++it) { - geometry_msgs::Point point; + geometry_msgs::msg::Point point; if ((*it).size() < 2) { ROS_WARN_STREAM("Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); From 3604a2e8d3abf8a5bfbd6a2457c1cf729f94b2cb Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 10:46:09 +0100 Subject: [PATCH 08/30] fix some more errors in logging and imports --- src/communication.cpp | 17 +++++++++-------- src/io.cpp | 8 +++++--- 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/communication.cpp b/src/communication.cpp index 44eeef6..785dc2a 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -1,12 +1,13 @@ #include "emc/communication.h" +#include +#include + //#include //#include #include #include -#include - namespace emc { @@ -42,16 +43,16 @@ Communication::Communication(std::string /*robot_name*/) : rclcpp::Node("emc_sys if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; */ - sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, _1)); - sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, _1)); - sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, _1)); - sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, _1)); + sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, std::placeholders::_1)); + sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, std::placeholders::_1)); + sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, std::placeholders::_1)); + sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, std::placeholders::_1)); pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); pub_open_door_ = this->create_publisher(open_door_param, 10); pub_speak_ = this->create_publisher(speak_param, 10); pub_play_ = this->create_publisher(play_param, 10); - pub_marker_ = this->create_publisher("/marker", 10); + pub_marker_ = this->create_publisher("/marker", 10); /* ros::SubscribeOptions laser_sub_options = ros::SubscribeOptions::create(laser_param, 1, boost::bind(&Communication::laserCallback, this, _1), ros::VoidPtr(), &laser_cb_queue_); @@ -126,7 +127,7 @@ bool Communication::readOdometryData(OdometryData& odom) odom.y = odom_msg_->pose.pose.position.y; // Calculate yaw rotation from quaternion - const geometry_msgs::Quaternion& q = odom_msg_->pose.pose.orientation; + const geometry_msgs::msg::Quaternion& q = odom_msg_->pose.pose.orientation; odom.a = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); odom.timestamp = odom_msg_->header.stamp.toSec(); diff --git a/src/io.cpp b/src/io.cpp index 72abde6..486360c 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -2,6 +2,8 @@ #include "emc/communication.h" +#include "rclcpp/rclcpp.hpp" + //#include // for ros::ok() #include #include @@ -106,7 +108,7 @@ bool IO::sendPath(std::vector> path, std::array c geometry_msgs::msg::Point point; if ((*it).size() < 2) { - ROS_WARN_STREAM("Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); + RCLCPP_WARN_STREAM(comm_->get_logger(), "Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); continue; } point.x = (*it)[0]; @@ -120,14 +122,14 @@ bool IO::sendPath(std::vector> path, std::array c point.z = (*it)[3]; if ((*it).size() > 3) { - ROS_WARN_STREAM("point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); + RCLCPP_WARN_STREAM(comm_->get_logger(), "point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); } } pathMarker.points.push_back(point); } if (pathMarker.points.size() < 2) { - ROS_ERROR_STREAM("Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); + RCLCPP_ERROR_STREAM(comm_->get_logger(), "Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); return false; } comm_->sendMarker(pathMarker); From f7fa7ba9b6a50339029930a41229cbe2b28dabe5 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 11:03:41 +0100 Subject: [PATCH 09/30] fix time syntax --- src/communication.cpp | 6 +++--- src/io.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/communication.cpp b/src/communication.cpp index 785dc2a..8c0ae22 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -108,7 +108,7 @@ bool Communication::readLaserData(LaserData& scan) scan.angle_min = laser_msg_->angle_min; scan.angle_max = laser_msg_->angle_max; scan.angle_increment = laser_msg_->angle_increment; - scan.timestamp = laser_msg_->header.stamp.toSec(); + scan.timestamp = rclcpp::Time(laser_msg_->header.stamp).seconds(); return true; } @@ -130,7 +130,7 @@ bool Communication::readOdometryData(OdometryData& odom) const geometry_msgs::msg::Quaternion& q = odom_msg_->pose.pose.orientation; odom.a = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); - odom.timestamp = odom_msg_->header.stamp.toSec(); + odom.timestamp = rclcpp::Time(odom_msg_->header.stamp).seconds(); return true; } @@ -212,7 +212,7 @@ void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) { // Publish tf transform geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = ros::Time::now(); + transformStamped.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); //rclcpp::Time::now(); transformStamped.header.frame_id = "map"; transformStamped.child_frame_id = robot_frame_name; transformStamped.transform = pose; diff --git a/src/io.cpp b/src/io.cpp index 486360c..47695b1 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -92,7 +92,7 @@ bool IO::sendPath(std::vector> path, std::array c { visualization_msgs::msg::Marker pathMarker; pathMarker.header.frame_id = "map"; - pathMarker.header.stamp = ros::Time(); + pathMarker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); pathMarker.ns = "path"; pathMarker.id = id; pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; From 2049185b9c2fe15b60934d62ddd5cae0fd4e10e4 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 11:18:25 +0100 Subject: [PATCH 10/30] fix tf publishing --- include/emc/communication.h | 9 +++------ src/communication.cpp | 5 +++-- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index 83bab8d..640ee57 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -64,10 +64,9 @@ class Communication : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_play_; rclcpp::Publisher::SharedPtr pub_marker_; - // Position data - - std::unique_ptr pub_tf2; //has to be defined after ros::init(), which is called in the constructor - + // pose publishing + std::string robot_frame_name; + std::unique_ptr pub_tf2_; //has to be defined after ros::init(), which is called in the constructor // Laser data rclcpp::Subscription::SharedPtr sub_laser_; @@ -95,8 +94,6 @@ class Communication : public rclcpp::Node void bumperfCallback(const std_msgs::msg::Bool::SharedPtr msg); void bumperbCallback(const std_msgs::msg::Bool::SharedPtr msg); - // pose publishing - std::string robot_frame_name; }; } // end namespace emc diff --git a/src/communication.cpp b/src/communication.cpp index 8c0ae22..4e604bb 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -76,7 +76,8 @@ Communication::Communication(std::string /*robot_name*/) : rclcpp::Node("emc_sys pub_marker_ = nh.advertise("/marker", 1); */ - pub_tf2 = std::unique_ptr(new tf2_ros::TransformBroadcaster); + pub_tf2_ = std::make_unique(*this); + //pub_tf2 = std::unique_ptr(new tf2_ros::TransformBroadcaster); } // ---------------------------------------------------------------------------------------------------- @@ -216,7 +217,7 @@ void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) transformStamped.header.frame_id = "map"; transformStamped.child_frame_id = robot_frame_name; transformStamped.transform = pose; - pub_tf2->sendTransform(transformStamped); + pub_tf2_->sendTransform(transformStamped); } // ---------------------------------------------------------------------------------------------------- From 5d6e1594de598c9dce125bb3a9fc2a30668ef929 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 11:19:04 +0100 Subject: [PATCH 11/30] fix errors in rate and fsm --- CMakeLists.txt | 4 ++-- include/emc/rate.h | 6 +++--- src/rate.cpp | 6 +++--- tools/visualize.cpp | 5 +++-- 4 files changed, 11 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4acc97d..9db7d92 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,8 +69,8 @@ target_link_libraries(testspeech emc_system) add_executable(emc_example1 examples/example01.cpp) target_link_libraries(emc_example1 emc_system) -add_executable(emc_example2 examples/example02.cpp) -target_link_libraries(emc_example2 emc_system) +#add_executable(emc_example2 examples/example02.cpp) +#target_link_libraries(emc_example2 emc_system) # ------------------------------------------------------------------------------------------------ # UNIT TESTS diff --git a/include/emc/rate.h b/include/emc/rate.h index 1484f35..01d1d53 100644 --- a/include/emc/rate.h +++ b/include/emc/rate.h @@ -12,14 +12,14 @@ class GenericRate; namespace emc { -class Rate2 +class Rate { public: - Rate2(double freq); + Rate(double freq); - ~Rate2(); + ~Rate(); void sleep(); diff --git a/src/rate.cpp b/src/rate.cpp index e361349..117b984 100644 --- a/src/rate.cpp +++ b/src/rate.cpp @@ -1,14 +1,14 @@ #include "emc/rate.h" -emc::Rate2::Rate2(double freq) +emc::Rate::Rate(double freq) { //rclcpp::Time::init(); rate_ = new rclcpp::Rate(freq); //logger_ = rclcpp::get_logger("ratelogger"); } -emc::Rate2::~Rate2() +emc::Rate::~Rate() { if (rate_) { @@ -16,7 +16,7 @@ emc::Rate2::~Rate2() } } -void emc::Rate2::sleep() +void emc::Rate::sleep() { if (!rate_->sleep()) { diff --git a/tools/visualize.cpp b/tools/visualize.cpp index 42753f8..a05a2b3 100644 --- a/tools/visualize.cpp +++ b/tools/visualize.cpp @@ -2,7 +2,8 @@ #include "opencv2/imgproc.hpp" #include -#include + +#include "rclcpp/rclcpp.hpp" #include @@ -29,7 +30,7 @@ int main(int argc, char **argv) emc::IO io(robot_name); - ros::Rate r(30); + rclcpp::Rate r(30); while(io.ok()) { cv::Mat canvas(500, 500, CV_8UC3, cv::Scalar(50, 50, 50)); From f413ba03bd34bf9667b9d0db7e10e44642207fb2 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 11:22:46 +0100 Subject: [PATCH 12/30] add visualization msgs dependency --- CMakeLists.txt | 3 ++- package.xml | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9db7d92..c874c84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ add_compile_options(-Wextra -Werror=extra) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) @@ -43,7 +44,7 @@ add_library(emc_system src/io.cpp src/rate.cpp ) -ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs rclcpp tf2 tf2_ros) +ament_target_dependencies(emc_system geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros) # ------------------------------------------------------------------------------------------------ # TOOLS diff --git a/package.xml b/package.xml index 9a847c5..0e317f2 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ nav_msgs rclcpp sensor_msgs + visualization_msgs tf2 tf2_ros From 01d7ecc2580775ab3ab9d0bf28beda39c465aa06 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 12:02:48 +0100 Subject: [PATCH 13/30] install executables --- CMakeLists.txt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c874c84..298e13b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,8 +60,8 @@ target_link_libraries(emc_viz emc_system ${OpenCV_LIBRARIES}) add_executable(emc_test_io examples/test_io.cpp) target_link_libraries(emc_test_io emc_system) -add_executable(testspeech examples/testspeech.cpp) -target_link_libraries(testspeech emc_system) +add_executable(emc_test_speech examples/testspeech.cpp) +target_link_libraries(emc_test_speech emc_system) # ------------------------------------------------------------------------------------------------ # EXAMPLES @@ -84,4 +84,11 @@ target_link_libraries(emc_example1 emc_system) # ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros rostest) #endif() +############# +## Install ## +############# +install(TARGETS emc_example1 emc_test_io emc_test_speech + DESTINATION lib/${PROJECT_NAME} +) + ament_package() From 4bd448328e1616c013ff108c3154e638b5fa17d3 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 12:18:21 +0100 Subject: [PATCH 14/30] initialise rcl --- src/io.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/io.cpp b/src/io.cpp index 47695b1..1c1896c 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -20,11 +20,16 @@ namespace emc IO::IO(Communication* comm) : comm_(comm) { if (!comm_) + { + rclcpp::init(0,nullptr); comm_ = new Communication; + } } -IO::IO(std::string robot_name) : comm_(new Communication(robot_name)) +IO::IO(std::string robot_name) { + rclcpp::init(0,nullptr); + comm_ = new Communication(robot_name); } IO::~IO() From 966fee9f90a6374e85824e9fb239c69b209c3245 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Wed, 28 Feb 2024 12:42:53 +0100 Subject: [PATCH 15/30] remove outdated comment --- include/emc/communication.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index 640ee57..84819d2 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -66,7 +66,7 @@ class Communication : public rclcpp::Node // pose publishing std::string robot_frame_name; - std::unique_ptr pub_tf2_; //has to be defined after ros::init(), which is called in the constructor + std::unique_ptr pub_tf2_; // Laser data rclcpp::Subscription::SharedPtr sub_laser_; From 4385e47d5bc57ae158af8a59890491f33d34f838 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Fri, 8 Mar 2024 10:17:19 +0100 Subject: [PATCH 16/30] update example for debugging --- examples/test_io.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/examples/test_io.cpp b/examples/test_io.cpp index c0359bf..9d01051 100644 --- a/examples/test_io.cpp +++ b/examples/test_io.cpp @@ -9,7 +9,7 @@ int main() emc::IO io; // Create Rate object, which will help using keeping the loop at a fixed frequency - emc::Rate r(100); + emc::Rate r(10); // Loop while we are properly connected while(io.ok()) @@ -20,10 +20,14 @@ int main() emc::OdometryData odom; if (io.readOdometryData(odom)) std::cout << "Odometry: " << odom.x << ", " << odom.y << ", " << odom.a << std::endl; + else + std::cout << "No Odom received" << std::endl; emc::LaserData scan; if (io.readLaserData(scan)) std::cout << "Laser: " << scan.ranges.size() << " beams" << std::endl; + else + std::cout << "No laser received" << std::endl; // Sleep remaining time r.sleep(); From 254bf976f8055a9d2ef8ceab355f8b39b495f277 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Fri, 8 Mar 2024 12:08:10 +0100 Subject: [PATCH 17/30] add ros2 publisher and subscriber wrappers --- CMakeLists.txt | 10 +++- examples/test_io.cpp | 3 + examples/test_publisher.cpp | 28 +++++++++ examples/test_subscriber.cpp | 36 ++++++++++++ include/emc/ros2publisher.h | 109 +++++++++++++++++++++++++++++++++++ include/emc/ros2subscriber.h | 44 ++++++++++++++ 6 files changed, 229 insertions(+), 1 deletion(-) create mode 100644 examples/test_publisher.cpp create mode 100644 examples/test_subscriber.cpp create mode 100644 include/emc/ros2publisher.h create mode 100644 include/emc/ros2subscriber.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 298e13b..b07f88f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,8 @@ add_library(emc_system include/emc/io.h include/emc/rate.h include/emc/bumper.h + include/emc/ros2subscriber.h + include/emc/ros2publisher.h #src/engine.cpp src/communication.cpp @@ -70,6 +72,12 @@ target_link_libraries(emc_test_speech emc_system) add_executable(emc_example1 examples/example01.cpp) target_link_libraries(emc_example1 emc_system) +add_executable(emc_test_subscriber examples/test_subscriber.cpp) +target_link_libraries(emc_test_subscriber emc_system) + +add_executable(emc_test_publisher examples/test_publisher.cpp) +target_link_libraries(emc_test_publisher emc_system) + #add_executable(emc_example2 examples/example02.cpp) #target_link_libraries(emc_example2 emc_system) @@ -87,7 +95,7 @@ target_link_libraries(emc_example1 emc_system) ############# ## Install ## ############# -install(TARGETS emc_example1 emc_test_io emc_test_speech +install(TARGETS emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/examples/test_io.cpp b/examples/test_io.cpp index 9d01051..abd8a98 100644 --- a/examples/test_io.cpp +++ b/examples/test_io.cpp @@ -3,8 +3,11 @@ #include +#include "rclcpp/rclcpp.hpp" + int main() { + rclcpp::init(0,nullptr); // Create IO object, which will initialize the io layer emc::IO io; diff --git a/examples/test_publisher.cpp b/examples/test_publisher.cpp new file mode 100644 index 0000000..1b876ad --- /dev/null +++ b/examples/test_publisher.cpp @@ -0,0 +1,28 @@ +#include "emc/ros2publisher.h" + +#include +#include "rclcpp/rclcpp.hpp" + +int main() +{ + rclcpp::init(0,nullptr); + + // Create publisher + emc::Ros2Publisher pub; + + // Create Rate object, which will help using keeping the loop at a fixed frequency + emc::Rate r(10); + + // Loop while we are properly connected + while(rclcpp::ok()) + { + std::cout << "sending msgs" << std::endl; + pub.sendBaseVelocity(1, 0, 0.1); + pub.speak("hello world"); + + // Sleep remaining time + r.sleep(); + } + + return 0; +} diff --git a/examples/test_subscriber.cpp b/examples/test_subscriber.cpp new file mode 100644 index 0000000..e51ad33 --- /dev/null +++ b/examples/test_subscriber.cpp @@ -0,0 +1,36 @@ +#include "emc/ros2subscriber.h" + +#include +#include "rclcpp/rclcpp.hpp" + +#include + +int main() +{ + rclcpp::init(0,nullptr); + + // Create subscriber + std::shared_ptr> laser_node = std::make_shared>("laser_scan"); + rclcpp::executors::SingleThreadedExecutor executor; + + executor.add_node(laser_node); + + // Create Rate object, which will help using keeping the loop at a fixed frequency + emc::Rate r(10); + + // Loop while we are properly connected + while(rclcpp::ok()) + { + std::cout << "spin" <readMsg(msg); + std::cout << "newdata: " << newdata << std::endl; + std::cout << "Laser: " << msg.range_min << " range_min" << std::endl; + + // Sleep remaining time + r.sleep(); + } + + return 0; +} diff --git a/include/emc/ros2publisher.h b/include/emc/ros2publisher.h new file mode 100644 index 0000000..685831b --- /dev/null +++ b/include/emc/ros2publisher.h @@ -0,0 +1,109 @@ +#ifndef EMC_ROS2PUBLISHER_H_ +#define EMC_ROS2PUBLISHER_H_ + +#include "rclcpp/rclcpp.hpp" + +#include + +#include +#include +#include +#include + +namespace emc +{ + +/** + * collection of all publishers used in the emc system +*/ +class Ros2Publisher : public rclcpp::Node +{ + +public: + + Ros2Publisher() : rclcpp::Node("emc_publishers") + { + std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; + laser_param = "laser_scan"; + odom_param = "odom"; + bumper_f_param = "bumper_f"; + bumper_b_param = "bumper_b"; + base_ref_param = "cmd_vel"; + open_door_param = "open_door"; + speak_param = "speak"; + play_param = "play"; + + pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); + pub_open_door_ = this->create_publisher(open_door_param, 10); + pub_speak_ = this->create_publisher(speak_param, 10); + pub_play_ = this->create_publisher(play_param, 10); + pub_marker_ = this->create_publisher("/marker", 10); + + pub_tf2_ = std::make_unique(*this); + }; + + void sendBaseVelocity(double vx, double vy, double va) + { + geometry_msgs::msg::Twist ref; + ref.linear.x = vx; + ref.linear.y = vy; + ref.angular.z = va; + + pub_cmd_vel_->publish(ref); + }; + + void sendOpendoorRequest() + { + std_msgs::msg::Empty msg; + pub_open_door_->publish(msg); + }; + + void sendMarker(visualization_msgs::msg::Marker marker) + { + pub_marker_->publish(marker); + }; + + void speak(const std::string& text) + { + std_msgs::msg::String str; + str.data = text; + pub_speak_->publish(str); + }; + + void play(const std::string& file) + { + std_msgs::msg::String str; + str.data = file; + pub_play_->publish(str); + }; + + // Postion data + void sendPoseEstimate(const geometry_msgs::msg::Transform& pose) + { + // Publish tf transform + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); //rclcpp::Time::now(); + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = robot_frame_name; + transformStamped.transform = pose; + pub_tf2_->sendTransform(transformStamped); + }; + + +private: + rclcpp::Publisher::SharedPtr pub_cmd_vel_; + rclcpp::Publisher::SharedPtr pub_open_door_; + rclcpp::Publisher::SharedPtr pub_speak_; + rclcpp::Publisher::SharedPtr pub_play_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // pose publishing + std::string robot_frame_name; + std::unique_ptr pub_tf2_; +}; + +} // end namespace emc + +#endif + + \ No newline at end of file diff --git a/include/emc/ros2subscriber.h b/include/emc/ros2subscriber.h new file mode 100644 index 0000000..46443ef --- /dev/null +++ b/include/emc/ros2subscriber.h @@ -0,0 +1,44 @@ +#ifndef EMC_ROS2SUBSCRIBER_H_ +#define EMC_ROS2SUBSCRIBER_H_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace emc +{ + +template +class Ros2Subscriber : public rclcpp::Node +{ + +public: + + Ros2Subscriber(std::string topic_name) : rclcpp::Node(topic_name) + { + sub_ = this->create_subscription(topic_name, 10, std::bind(&Ros2Subscriber::callback, this, std::placeholders::_1)); + }; + + bool readMsg(T& msg) + { + if (!msg_) + return false; + msg = *msg_; + msg_.reset(); + return true; + }; + +private: + typename rclcpp::Subscription::SharedPtr sub_; + + typename T::SharedPtr msg_; + + void callback(const typename T::SharedPtr msg){ + msg_ = msg; + }; +}; + +} // end namespace emc + +#endif From 2d3b3babe2588638654847f9c1780d4b73cf8a5b Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Fri, 8 Mar 2024 13:31:40 +0100 Subject: [PATCH 18/30] integrate pub and sub wrappers in communication --- examples/test_io.cpp | 3 - include/emc/communication.h | 52 +++-------- include/emc/ros2publisher.h | 50 +++++++++- src/communication.cpp | 180 +++++++++--------------------------- src/io.cpp | 64 ++----------- 5 files changed, 114 insertions(+), 235 deletions(-) diff --git a/examples/test_io.cpp b/examples/test_io.cpp index abd8a98..9d01051 100644 --- a/examples/test_io.cpp +++ b/examples/test_io.cpp @@ -3,11 +3,8 @@ #include -#include "rclcpp/rclcpp.hpp" - int main() { - rclcpp::init(0,nullptr); // Create IO object, which will initialize the io layer emc::IO io; diff --git a/include/emc/communication.h b/include/emc/communication.h index 84819d2..5a900a8 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -4,6 +4,8 @@ #include "emc/data.h" #include "emc/odom.h" #include "emc/bumper.h" +#include "emc/ros2publisher.h" +#include "emc/ros2subscriber.h" #include "rclcpp/rclcpp.hpp" //#include @@ -24,7 +26,7 @@ namespace emc { -class Communication : public rclcpp::Node +class Communication { public: @@ -44,7 +46,7 @@ class Communication : public rclcpp::Node void sendBaseVelocity(double vx, double vy, double va); - void sendOpendoorRequest(); + void sendOpenDoorRequest(); void sendMarker(visualization_msgs::msg::Marker marker); @@ -55,45 +57,19 @@ class Communication : public rclcpp::Node // Postion data void sendPoseEstimate(const geometry_msgs::msg::Transform& pose); -private: - - // Base velocity reference - rclcpp::Publisher::SharedPtr pub_cmd_vel_; - rclcpp::Publisher::SharedPtr pub_open_door_; - rclcpp::Publisher::SharedPtr pub_speak_; - rclcpp::Publisher::SharedPtr pub_play_; - rclcpp::Publisher::SharedPtr pub_marker_; - - // pose publishing - std::string robot_frame_name; - std::unique_ptr pub_tf2_; - - // Laser data - rclcpp::Subscription::SharedPtr sub_laser_; - - sensor_msgs::msg::LaserScan::SharedPtr laser_msg_; - - void laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg); - + bool sendPath(std::vector> path, std::array color, double width, int id) + { + return pub_node_->sendPath(path, color, width, id); + } - // Odometry data - - rclcpp::Subscription::SharedPtr sub_odom_; - - nav_msgs::msg::Odometry::SharedPtr odom_msg_; - - void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg); - - // Bumper data - rclcpp::Subscription::SharedPtr sub_bumper_f_; - rclcpp::Subscription::SharedPtr sub_bumper_b_; - - std_msgs::msg::Bool::SharedPtr bumper_f_msg_; - std_msgs::msg::Bool::SharedPtr bumper_b_msg_; +private: + Ros2Publisher* pub_node_; - void bumperfCallback(const std_msgs::msg::Bool::SharedPtr msg); - void bumperbCallback(const std_msgs::msg::Bool::SharedPtr msg); + std::shared_ptr> laser_node_; + rclcpp::executors::SingleThreadedExecutor* laser_executor_; + std::shared_ptr> odom_node_; + rclcpp::executors::SingleThreadedExecutor* odom_executor_; }; } // end namespace emc diff --git a/include/emc/ros2publisher.h b/include/emc/ros2publisher.h index 685831b..ac34e15 100644 --- a/include/emc/ros2publisher.h +++ b/include/emc/ros2publisher.h @@ -52,7 +52,7 @@ class Ros2Publisher : public rclcpp::Node pub_cmd_vel_->publish(ref); }; - void sendOpendoorRequest() + void sendOpenDoorRequest() { std_msgs::msg::Empty msg; pub_open_door_->publish(msg); @@ -89,6 +89,54 @@ class Ros2Publisher : public rclcpp::Node pub_tf2_->sendTransform(transformStamped); }; + bool sendPath(std::vector> path, std::array color, double width, int id) + { + visualization_msgs::msg::Marker pathMarker; + pathMarker.header.frame_id = "map"; + pathMarker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + pathMarker.ns = "path"; + pathMarker.id = id; + pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; + pathMarker.action = visualization_msgs::msg::Marker::ADD; + pathMarker.color.a = 1.0; + pathMarker.color.r = color[0]; + pathMarker.color.g = color[1]; + pathMarker.color.b = color[2]; + pathMarker.pose.orientation.w = 1.0; + pathMarker.scale.x = width; + for (std::vector>::iterator it = path.begin(); it != path.end(); ++it) + { + geometry_msgs::msg::Point point; + if ((*it).size() < 2) + { + RCLCPP_WARN_STREAM(this->get_logger(), "Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); + continue; + } + point.x = (*it)[0]; + point.y = (*it)[1]; + if ((*it).size() == 2) + { + point.z = 0; + } + else + { + point.z = (*it)[3]; + if ((*it).size() > 3) + { + RCLCPP_WARN_STREAM(this->get_logger(), "point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); + } + } + pathMarker.points.push_back(point); + } + if (pathMarker.points.size() < 2) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); + return false; + } + sendMarker(pathMarker); + return true; + }; + private: rclcpp::Publisher::SharedPtr pub_cmd_vel_; diff --git a/src/communication.cpp b/src/communication.cpp index 4e604bb..1a72c47 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -3,24 +3,17 @@ #include #include -//#include -//#include #include #include namespace emc { -// ---------------------------------------------------------------------------------------------------- - -Communication::Communication(std::string /*robot_name*/) : rclcpp::Node("emc_system") +Communication::Communication(std::string /*robot_name*/) { -/* - ros::VP_string args; - ros::init(args, "emc_system", ros::init_options::AnonymousName); - ros::Time::init(); -*/ - // ros::NodeHandle nh; + std::cout << "constructor of Communication" << std::endl; + rclcpp::init(0,nullptr); + std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; // temp hardcode param names laser_param = "laser_scan"; @@ -43,103 +36,70 @@ Communication::Communication(std::string /*robot_name*/) : rclcpp::Node("emc_sys if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; */ - sub_laser_ = this->create_subscription(laser_param, 10, std::bind(&Communication::laserCallback, this, std::placeholders::_1)); - sub_odom_ = this->create_subscription(odom_param, 10, std::bind(&Communication::odomCallback, this, std::placeholders::_1)); - sub_bumper_f_ = this->create_subscription(bumper_f_param, 10, std::bind(&Communication::bumperfCallback, this, std::placeholders::_1)); - sub_bumper_b_ = this->create_subscription(bumper_b_param, 10, std::bind(&Communication::bumperbCallback, this, std::placeholders::_1)); - - pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); - pub_open_door_ = this->create_publisher(open_door_param, 10); - pub_speak_ = this->create_publisher(speak_param, 10); - pub_play_ = this->create_publisher(play_param, 10); - pub_marker_ = this->create_publisher("/marker", 10); - -/* - ros::SubscribeOptions laser_sub_options = ros::SubscribeOptions::create(laser_param, 1, boost::bind(&Communication::laserCallback, this, _1), ros::VoidPtr(), &laser_cb_queue_); - sub_laser_ = nh.subscribe(laser_sub_options); - - ros::SubscribeOptions odom_sub_options = ros::SubscribeOptions::create(odom_param, 1, boost::bind(&Communication::odomCallback, this, _1), ros::VoidPtr(), &odom_cb_queue_); - sub_odom_ = nh.subscribe(odom_sub_options); - ros::SubscribeOptions bumper_f_sub_options = ros::SubscribeOptions::create(bumper_f_param, 1, boost::bind(&Communication::bumperfCallback, this, _1), ros::VoidPtr(), &bumper_f_cb_queue_); - sub_bumper_f_ = nh.subscribe(bumper_f_sub_options); + laser_node_ = std::make_shared>(laser_param); + laser_executor_ = new rclcpp::executors::SingleThreadedExecutor; + laser_executor_->add_node(laser_node_); - ros::SubscribeOptions bumper_b_sub_options = ros::SubscribeOptions::create(bumper_b_param, 1, boost::bind(&Communication::bumperbCallback, this, _1), ros::VoidPtr(), &bumper_b_cb_queue_); - sub_bumper_b_ = nh.subscribe(bumper_b_sub_options); + odom_node_ = std::make_shared>(odom_param); + odom_executor_ = new rclcpp::executors::SingleThreadedExecutor; + odom_executor_->add_node(odom_node_); - pub_base_ref_ = nh.advertise(base_ref_param, 1); - - pub_open_door_ = nh.advertise(open_door_param, 1); - - pub_speak_ = nh.advertise(speak_param, 1); - pub_play_ = nh.advertise(play_param, 1); - - pub_marker_ = nh.advertise("/marker", 1); -*/ - pub_tf2_ = std::make_unique(*this); - //pub_tf2 = std::unique_ptr(new tf2_ros::TransformBroadcaster); + pub_node_ = new Ros2Publisher(); } -// ---------------------------------------------------------------------------------------------------- - Communication::~Communication() { } -// ---------------------------------------------------------------------------------------------------- - void Communication::init() { } -// ---------------------------------------------------------------------------------------------------- - bool Communication::readLaserData(LaserData& scan) { - laser_msg_.reset(); - //laser_cb_queue_.callAvailable(); + laser_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. - if (!laser_msg_) + sensor_msgs::msg::LaserScan msg; + if(!laser_node_->readMsg(msg)) return false; - scan.range_min = laser_msg_->range_min; - scan.range_max = laser_msg_->range_max; - scan.ranges = laser_msg_->ranges; - scan.angle_min = laser_msg_->angle_min; - scan.angle_max = laser_msg_->angle_max; - scan.angle_increment = laser_msg_->angle_increment; - scan.timestamp = rclcpp::Time(laser_msg_->header.stamp).seconds(); + scan.range_min = msg.range_min; + scan.range_max = msg.range_max; + scan.ranges = msg.ranges; + scan.angle_min = msg.angle_min; + scan.angle_max = msg.angle_max; + scan.angle_increment = msg.angle_increment; + scan.timestamp = rclcpp::Time(msg.header.stamp).seconds(); return true; } -// ---------------------------------------------------------------------------------------------------- - bool Communication::readOdometryData(OdometryData& odom) { - odom_msg_.reset(); - //odom_cb_queue_.callAvailable(); - - if (!odom_msg_) + odom_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. + + nav_msgs::msg::Odometry msg; + if(!odom_node_->readMsg(msg)) return false; - - odom.x = odom_msg_->pose.pose.position.x; - odom.y = odom_msg_->pose.pose.position.y; + + odom.x = msg.pose.pose.position.x; + odom.y = msg.pose.pose.position.y; // Calculate yaw rotation from quaternion - const geometry_msgs::msg::Quaternion& q = odom_msg_->pose.pose.orientation; + const geometry_msgs::msg::Quaternion& q = msg.pose.pose.orientation; odom.a = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); - odom.timestamp = rclcpp::Time(odom_msg_->header.stamp).seconds(); + odom.timestamp = rclcpp::Time(msg.header.stamp).seconds(); return true; } -// ---------------------------------------------------------------------------------------------------- - -bool Communication::readFrontBumperData(BumperData& bumper) +bool Communication::readFrontBumperData(BumperData& /*bumper*/) { + return false; + /* bumper_f_msg_.reset(); //bumper_f_cb_queue_.callAvailable(); @@ -149,12 +109,13 @@ bool Communication::readFrontBumperData(BumperData& bumper) bumper.contact = bumper_f_msg_->data; return true; + */ } -// ---------------------------------------------------------------------------------------------------- - -bool Communication::readBackBumperData(BumperData& bumper) +bool Communication::readBackBumperData(BumperData& /*bumper*/) { + return false; + /* bumper_b_msg_.reset(); //bumper_b_cb_queue_.callAvailable(); @@ -164,88 +125,37 @@ bool Communication::readBackBumperData(BumperData& bumper) bumper.contact = bumper_b_msg_->data; return true; + */ } -// ---------------------------------------------------------------------------------------------------- - void Communication::sendBaseVelocity(double vx, double vy, double va) { - geometry_msgs::msg::Twist ref; - ref.linear.x = vx; - ref.linear.y = vy; - ref.angular.z = va; - - pub_cmd_vel_->publish(ref); + pub_node_->sendBaseVelocity(vx,vy, va); } -// ---------------------------------------------------------------------------------------------------- - -void Communication::sendOpendoorRequest() +void Communication::sendOpenDoorRequest() { - std_msgs::msg::Empty msg; - pub_open_door_->publish(msg); + pub_node_->sendOpenDoorRequest(); } -// ---------------------------------------------------------------------------------------------------- - void Communication::sendMarker(visualization_msgs::msg::Marker marker) { - pub_marker_->publish(marker); + pub_node_->sendMarker(marker); } -// ---------------------------------------------------------------------------------------------------- - void Communication::speak(const std::string& text) { - std_msgs::msg::String str; - str.data = text; - pub_speak_->publish(str); + pub_node_->speak(text); } void Communication::play(const std::string& file) { - std_msgs::msg::String str; - str.data = file; - pub_play_->publish(str); + pub_node_->play(file); } void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) { - // Publish tf transform - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); //rclcpp::Time::now(); - transformStamped.header.frame_id = "map"; - transformStamped.child_frame_id = robot_frame_name; - transformStamped.transform = pose; - pub_tf2_->sendTransform(transformStamped); -} - -// ---------------------------------------------------------------------------------------------------- - -void Communication::laserCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) -{ - laser_msg_ = msg; -} - -// ---------------------------------------------------------------------------------------------------- - -void Communication::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - odom_msg_ = msg; -} - -// ---------------------------------------------------------------------------------------------------- - -void Communication::bumperfCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - bumper_f_msg_ = msg; -} - -// ---------------------------------------------------------------------------------------------------- - -void Communication::bumperbCallback(const std_msgs::msg::Bool::SharedPtr msg) -{ - bumper_b_msg_ = msg; + pub_node_->sendPoseEstimate(pose); } } // end namespace emc diff --git a/src/io.cpp b/src/io.cpp index 1c1896c..60e4ce9 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -2,15 +2,11 @@ #include "emc/communication.h" -#include "rclcpp/rclcpp.hpp" +#include "rclcpp/rclcpp.hpp" // for rclcpp::ok() -//#include // for ros::ok() #include #include -#include -#include -#include -#include + #include @@ -19,16 +15,16 @@ namespace emc IO::IO(Communication* comm) : comm_(comm) { + std::cout << "constructor of io" << std::endl; if (!comm_) { - rclcpp::init(0,nullptr); + std::cout << "constructing comm_" << std::endl; comm_ = new Communication; } } IO::IO(std::string robot_name) { - rclcpp::init(0,nullptr); comm_ = new Communication(robot_name); } @@ -70,16 +66,11 @@ void IO::sendOpendoorRequest() system("aplay --device front:CARD=Device_1,DEV=0 ~/.emc/system/src/emc_system/sounds/doorbell.wav &"); #pragma GCC diagnostic pop - comm_->sendOpendoorRequest(); + comm_->sendOpenDoorRequest(); } void IO::speak(const std::string& text) { - //std::string cmd; - //cmd = "sudo espeak " + text + " --stdout | sudo aplay --device \"default:CARD=Device\""; - //cmd = "espeak " + text + " --stdout | aplay --device \"default:CARD=Device\" &"; - //system(cmd.c_str()); - comm_->speak(text); } @@ -95,50 +86,7 @@ bool IO::ok() bool IO::sendPath(std::vector> path, std::array color, double width, int id) { - visualization_msgs::msg::Marker pathMarker; - pathMarker.header.frame_id = "map"; - pathMarker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); - pathMarker.ns = "path"; - pathMarker.id = id; - pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; - pathMarker.action = visualization_msgs::msg::Marker::ADD; - pathMarker.color.a = 1.0; - pathMarker.color.r = color[0]; - pathMarker.color.g = color[1]; - pathMarker.color.b = color[2]; - pathMarker.pose.orientation.w = 1.0; - pathMarker.scale.x = width; - for (std::vector>::iterator it = path.begin(); it != path.end(); ++it) - { - geometry_msgs::msg::Point point; - if ((*it).size() < 2) - { - RCLCPP_WARN_STREAM(comm_->get_logger(), "Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); - continue; - } - point.x = (*it)[0]; - point.y = (*it)[1]; - if ((*it).size() == 2) - { - point.z = 0; - } - else - { - point.z = (*it)[3]; - if ((*it).size() > 3) - { - RCLCPP_WARN_STREAM(comm_->get_logger(), "point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); - } - } - pathMarker.points.push_back(point); - } - if (pathMarker.points.size() < 2) - { - RCLCPP_ERROR_STREAM(comm_->get_logger(), "Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); - return false; - } - comm_->sendMarker(pathMarker); - return true; + return comm_->sendPath(path, color, width, id); } bool IO::sendPoseEstimate(double px, double py, double pz, double rx, double ry, double rz, double rw) From bf373dc3b146831ebeacf5aa9b513a92f6e6a332 Mon Sep 17 00:00:00 2001 From: Matthijs van der Burgh Date: Fri, 8 Mar 2024 13:37:28 +0100 Subject: [PATCH 19/30] Fix missing newline at EOF --- include/emc/ros2publisher.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/emc/ros2publisher.h b/include/emc/ros2publisher.h index ac34e15..cca0459 100644 --- a/include/emc/ros2publisher.h +++ b/include/emc/ros2publisher.h @@ -153,5 +153,3 @@ class Ros2Publisher : public rclcpp::Node } // end namespace emc #endif - - \ No newline at end of file From cd962f066c289538d79206dc0369514a8b65ac50 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Fri, 8 Mar 2024 14:21:49 +0100 Subject: [PATCH 20/30] add node name as argument --- examples/test_subscriber.cpp | 2 +- include/emc/ros2subscriber.h | 2 +- src/communication.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/examples/test_subscriber.cpp b/examples/test_subscriber.cpp index e51ad33..9f44904 100644 --- a/examples/test_subscriber.cpp +++ b/examples/test_subscriber.cpp @@ -10,7 +10,7 @@ int main() rclcpp::init(0,nullptr); // Create subscriber - std::shared_ptr> laser_node = std::make_shared>("laser_scan"); + std::shared_ptr> laser_node = std::make_shared>("laser_scan", "emc_laser"); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(laser_node); diff --git a/include/emc/ros2subscriber.h b/include/emc/ros2subscriber.h index 46443ef..1d42193 100644 --- a/include/emc/ros2subscriber.h +++ b/include/emc/ros2subscriber.h @@ -15,7 +15,7 @@ class Ros2Subscriber : public rclcpp::Node public: - Ros2Subscriber(std::string topic_name) : rclcpp::Node(topic_name) + Ros2Subscriber(std::string topic_name, std::string node_name) : rclcpp::Node(node_name) { sub_ = this->create_subscription(topic_name, 10, std::bind(&Ros2Subscriber::callback, this, std::placeholders::_1)); }; diff --git a/src/communication.cpp b/src/communication.cpp index 1a72c47..b32514e 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -16,8 +16,8 @@ Communication::Communication(std::string /*robot_name*/) std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; // temp hardcode param names - laser_param = "laser_scan"; - odom_param = "odom"; + laser_param = "scan"; + odom_param = "odometry/filtered"; bumper_f_param = "bumper_f"; bumper_b_param = "bumper_b"; base_ref_param = "cmd_vel"; @@ -37,11 +37,11 @@ Communication::Communication(std::string /*robot_name*/) if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; */ - laser_node_ = std::make_shared>(laser_param); + laser_node_ = std::make_shared>(laser_param, "emc_laser"); laser_executor_ = new rclcpp::executors::SingleThreadedExecutor; laser_executor_->add_node(laser_node_); - odom_node_ = std::make_shared>(odom_param); + odom_node_ = std::make_shared>(odom_param, "emc_odom"); odom_executor_ = new rclcpp::executors::SingleThreadedExecutor; odom_executor_->add_node(odom_node_); From ec7663b1e1da9612d64cc50ce7a454695ced64e8 Mon Sep 17 00:00:00 2001 From: K de Vos Date: Thu, 11 Apr 2024 13:15:55 +0200 Subject: [PATCH 21/30] added publishers for visualizing the localization exercises --- include/emc/communication.h | 5 + include/emc/io.h | 6 + include/emc/ros2publisher.h | 323 +++++++++++++++++++++++------------- src/communication.cpp | 17 ++ src/io.cpp | 223 +++++++++++++------------ 5 files changed, 353 insertions(+), 221 deletions(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index 5a900a8..e2df203 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -62,6 +62,11 @@ class Communication return pub_node_->sendPath(path, color, width, id); } + // publishers used to visualize information in the localization exercises (particle filter): + void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); + void send_particles(int N, std::vector> particle_poses, double mapOrientation); + void send_pose(std::vector pose, double mapOrientation); + private: Ros2Publisher* pub_node_; diff --git a/include/emc/io.h b/include/emc/io.h index a51254b..ef8cc6b 100644 --- a/include/emc/io.h +++ b/include/emc/io.h @@ -128,6 +128,12 @@ class IO */ bool sendPoseEstimate(double x, double y, double yaw); + // publishers used to visualize information in the localization exercises (particle filter): + void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); + void send_particles(int N, std::vector> particle_poses, double mapOrientation); + void send_pose(std::vector pose, double mapOrientation); + + private: /** diff --git a/include/emc/ros2publisher.h b/include/emc/ros2publisher.h index cca0459..341b2db 100644 --- a/include/emc/ros2publisher.h +++ b/include/emc/ros2publisher.h @@ -4,151 +4,240 @@ #include "rclcpp/rclcpp.hpp" #include +#include "tf2/LinearMath/Quaternion.h" #include #include #include #include +#include +#include +#include namespace emc { -/** - * collection of all publishers used in the emc system -*/ -class Ros2Publisher : public rclcpp::Node -{ - -public: - - Ros2Publisher() : rclcpp::Node("emc_publishers") + /** + * collection of all publishers used in the emc system + */ + class Ros2Publisher : public rclcpp::Node { - std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; - laser_param = "laser_scan"; - odom_param = "odom"; - bumper_f_param = "bumper_f"; - bumper_b_param = "bumper_b"; - base_ref_param = "cmd_vel"; - open_door_param = "open_door"; - speak_param = "speak"; - play_param = "play"; - - pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); - pub_open_door_ = this->create_publisher(open_door_param, 10); - pub_speak_ = this->create_publisher(speak_param, 10); - pub_play_ = this->create_publisher(play_param, 10); - pub_marker_ = this->create_publisher("/marker", 10); - - pub_tf2_ = std::make_unique(*this); - }; - void sendBaseVelocity(double vx, double vy, double va) - { - geometry_msgs::msg::Twist ref; - ref.linear.x = vx; - ref.linear.y = vy; - ref.angular.z = va; + public: + Ros2Publisher() : rclcpp::Node("emc_publishers") + { + std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; + laser_param = "laser_scan"; + odom_param = "odom"; + bumper_f_param = "bumper_f"; + bumper_b_param = "bumper_b"; + base_ref_param = "cmd_vel"; + open_door_param = "open_door"; + speak_param = "speak"; + play_param = "play"; + + pub_cmd_vel_ = this->create_publisher(base_ref_param, 10); + pub_open_door_ = this->create_publisher(open_door_param, 10); + pub_speak_ = this->create_publisher(speak_param, 10); + pub_play_ = this->create_publisher(play_param, 10); + pub_marker_ = this->create_publisher("/marker", 10); + + // publishers used to visualize information in the localization exercises (particle filter): + this->pub_laser_msg = this->create_publisher("/laser_match", 1); + this->pub_particle = this->create_publisher("/particles", 1); + this->pub_pose = this->create_publisher("/pose_estimate", 1); + + pub_tf2_ = std::make_unique(*this); + }; + + void sendBaseVelocity(double vx, double vy, double va) + { + geometry_msgs::msg::Twist ref; + ref.linear.x = vx; + ref.linear.y = vy; + ref.angular.z = va; - pub_cmd_vel_->publish(ref); - }; + pub_cmd_vel_->publish(ref); + }; - void sendOpenDoorRequest() - { - std_msgs::msg::Empty msg; - pub_open_door_->publish(msg); - }; + void sendOpenDoorRequest() + { + std_msgs::msg::Empty msg; + pub_open_door_->publish(msg); + }; - void sendMarker(visualization_msgs::msg::Marker marker) - { - pub_marker_->publish(marker); - }; + void sendMarker(visualization_msgs::msg::Marker marker) + { + pub_marker_->publish(marker); + }; - void speak(const std::string& text) - { - std_msgs::msg::String str; - str.data = text; - pub_speak_->publish(str); - }; - - void play(const std::string& file) - { - std_msgs::msg::String str; - str.data = file; - pub_play_->publish(str); - }; + void speak(const std::string &text) + { + std_msgs::msg::String str; + str.data = text; + pub_speak_->publish(str); + }; - // Postion data - void sendPoseEstimate(const geometry_msgs::msg::Transform& pose) - { - // Publish tf transform - geometry_msgs::msg::TransformStamped transformStamped; - transformStamped.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); //rclcpp::Time::now(); - transformStamped.header.frame_id = "map"; - transformStamped.child_frame_id = robot_frame_name; - transformStamped.transform = pose; - pub_tf2_->sendTransform(transformStamped); - }; + void play(const std::string &file) + { + std_msgs::msg::String str; + str.data = file; + pub_play_->publish(str); + }; - bool sendPath(std::vector> path, std::array color, double width, int id) - { - visualization_msgs::msg::Marker pathMarker; - pathMarker.header.frame_id = "map"; - pathMarker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); - pathMarker.ns = "path"; - pathMarker.id = id; - pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; - pathMarker.action = visualization_msgs::msg::Marker::ADD; - pathMarker.color.a = 1.0; - pathMarker.color.r = color[0]; - pathMarker.color.g = color[1]; - pathMarker.color.b = color[2]; - pathMarker.pose.orientation.w = 1.0; - pathMarker.scale.x = width; - for (std::vector>::iterator it = path.begin(); it != path.end(); ++it) + // Postion data + void sendPoseEstimate(const geometry_msgs::msg::Transform &pose) + { + // Publish tf transform + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); // rclcpp::Time::now(); + transformStamped.header.frame_id = "map"; + transformStamped.child_frame_id = robot_frame_name; + transformStamped.transform = pose; + pub_tf2_->sendTransform(transformStamped); + }; + + bool sendPath(std::vector> path, std::array color, double width, int id) { - geometry_msgs::msg::Point point; - if ((*it).size() < 2) + visualization_msgs::msg::Marker pathMarker; + pathMarker.header.frame_id = "map"; + pathMarker.header.stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + pathMarker.ns = "path"; + pathMarker.id = id; + pathMarker.type = visualization_msgs::msg::Marker::LINE_STRIP; + pathMarker.action = visualization_msgs::msg::Marker::ADD; + pathMarker.color.a = 1.0; + pathMarker.color.r = color[0]; + pathMarker.color.g = color[1]; + pathMarker.color.b = color[2]; + pathMarker.pose.orientation.w = 1.0; + pathMarker.scale.x = width; + for (std::vector>::iterator it = path.begin(); it != path.end(); ++it) { - RCLCPP_WARN_STREAM(this->get_logger(), "Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); - continue; + geometry_msgs::msg::Point point; + if ((*it).size() < 2) + { + RCLCPP_WARN_STREAM(this->get_logger(), "Point at index " << std::distance(path.begin(), it) << " has too few dimensions (expected at least 2, got " << (*it).size() << "), skipping."); + continue; + } + point.x = (*it)[0]; + point.y = (*it)[1]; + if ((*it).size() == 2) + { + point.z = 0; + } + else + { + point.z = (*it)[3]; + if ((*it).size() > 3) + { + RCLCPP_WARN_STREAM(this->get_logger(), "point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); + } + } + pathMarker.points.push_back(point); } - point.x = (*it)[0]; - point.y = (*it)[1]; - if ((*it).size() == 2) + if (pathMarker.points.size() < 2) { - point.z = 0; + RCLCPP_ERROR_STREAM(this->get_logger(), "Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); + return false; } - else + sendMarker(pathMarker); + return true; + }; + + // publishers used to visualize information in the localization exercises (particle filter): + + void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + { + sensor_msgs::msg::LaserScan msg{}; + + msg.angle_min = angle_min; + msg.angle_max = angle_max; + msg.angle_increment = angle_inc * subsample; + + msg.range_min = 0.01; + msg.range_max = 10; + + msg.header.frame_id = "internal/base_link"; + msg.header.stamp = this->now(); + + msg.ranges = prediction; + + this->pub_laser_msg->publish(msg); + }; + + void send_particles(int N, std::vector> particle_poses, double mapOrientation) + { + geometry_msgs::msg::PoseArray msg{}; + + msg.header.frame_id = "map"; + msg.header.stamp = this->now(); + tf2::Quaternion a; + msg.poses.reserve(N); + for (int i = 0; i < N; i++) { - point.z = (*it)[3]; - if ((*it).size() > 3) - { - RCLCPP_WARN_STREAM(this->get_logger(), "point at index " << std::distance(path.begin(), it) << " has unused dimensions (expected 2 or 3, got " << (*it).size() << ")."); - } + geometry_msgs::msg::Pose posemsg; + auto pose_i = particle_poses[i]; + + posemsg.position.x = std::cos(mapOrientation) * pose_i[0] - std::sin(mapOrientation) * pose_i[1]; + posemsg.position.y = std::sin(mapOrientation) * pose_i[0] + std::cos(mapOrientation) * pose_i[1]; + posemsg.position.z = 0; + + a.setRPY(0, 0, pose_i[2] - mapOrientation); + posemsg.orientation.w = a.getW(); + posemsg.orientation.x = a.getX(); + posemsg.orientation.y = a.getY(); + posemsg.orientation.z = a.getZ(); + + msg.poses.push_back(posemsg); } - pathMarker.points.push_back(point); - } - if (pathMarker.points.size() < 2) + + this->pub_particle->publish(msg); + }; + + + void send_pose(std::vector pose, double mapOrientation) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Not enough valid points (expected at least 2, got " << pathMarker.points.size() << ")."); - return false; - } - sendMarker(pathMarker); - return true; - }; + geometry_msgs::msg::PoseArray msg; + msg.header.frame_id = "map"; + msg.header.stamp = this->now(); + tf2::Quaternion a; -private: - rclcpp::Publisher::SharedPtr pub_cmd_vel_; - rclcpp::Publisher::SharedPtr pub_open_door_; - rclcpp::Publisher::SharedPtr pub_speak_; - rclcpp::Publisher::SharedPtr pub_play_; - rclcpp::Publisher::SharedPtr pub_marker_; + msg.poses.reserve(1); - // pose publishing - std::string robot_frame_name; - std::unique_ptr pub_tf2_; -}; + geometry_msgs::msg::Pose posemsg; + posemsg.position.x = std::cos(mapOrientation) * pose[0] - std::sin(mapOrientation) * pose[1]; + posemsg.position.y = std::sin(mapOrientation) * pose[0] + std::cos(mapOrientation) * pose[1]; + posemsg.position.z = 0; + + a.setRPY(0, 0, pose[2] - mapOrientation); + posemsg.orientation.w = a.getW(); + posemsg.orientation.x = a.getX(); + posemsg.orientation.y = a.getY(); + posemsg.orientation.z = a.getZ(); + + msg.poses.push_back(posemsg); + + this->pub_pose->publish(msg); + }; + + private: + rclcpp::Publisher::SharedPtr pub_cmd_vel_; + rclcpp::Publisher::SharedPtr pub_open_door_; + rclcpp::Publisher::SharedPtr pub_speak_; + rclcpp::Publisher::SharedPtr pub_play_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // pose publishing + std::string robot_frame_name; + std::unique_ptr pub_tf2_; + + // publishers used to visualize information in the localization exercises (particle filter): + rclcpp::Publisher::SharedPtr pub_laser_msg; + rclcpp::Publisher::SharedPtr pub_particle; + rclcpp::Publisher::SharedPtr pub_pose; + }; } // end namespace emc diff --git a/src/communication.cpp b/src/communication.cpp index b32514e..7060d3a 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -158,5 +158,22 @@ void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) pub_node_->sendPoseEstimate(pose); } +// publishers used to visualize information in the localization exercises (particle filter): + + void Communication::send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + { + pub_node_->send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); + } + + void Communication::send_particles(int N, std::vector> particle_poses, double mapOrientation) + { + pub_node_->send_particles(N, particle_poses, mapOrientation); + } + + void Communication::send_pose(std::vector pose, double mapOrientation) + { + pub_node_->send_pose(pose, mapOrientation); + } + } // end namespace emc diff --git a/src/io.cpp b/src/io.cpp index f5b5e3e..6611b46 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -7,140 +7,155 @@ #include #include - #include namespace emc { -IO::IO(Communication* comm) : comm_(comm) -{ - std::cout << "constructor of io" << std::endl; - if (!comm_) + IO::IO(Communication *comm) : comm_(comm) { - std::cout << "constructing comm_" << std::endl; - comm_ = new Communication; + std::cout << "constructor of io" << std::endl; + if (!comm_) + { + std::cout << "constructing comm_" << std::endl; + comm_ = new Communication; + } } -} -IO::IO(std::string robot_name) -{ - comm_ = new Communication(robot_name); -} + IO::IO(std::string robot_name) + { + comm_ = new Communication(robot_name); + } -IO::~IO() -{ - if (comm_) - delete comm_; -} + IO::~IO() + { + if (comm_) + delete comm_; + } -bool IO::readLaserData(LaserData& scan) -{ - return comm_->readLaserData(scan); -} + bool IO::readLaserData(LaserData &scan) + { + return comm_->readLaserData(scan); + } -bool IO::readOdometryData(OdometryData& odom) -{ - OdometryData new_odom; - if (!comm_->readOdometryData(new_odom)) - return false; + bool IO::readOdometryData(OdometryData &odom) + { + OdometryData new_odom; + if (!comm_->readOdometryData(new_odom)) + return false; + + if (!odom_set_) + { + prev_odom_ = new_odom; + odom_set_ = true; + return false; + } + + odom.timestamp = new_odom.timestamp; // TODO give dt? + double dx = new_odom.x - prev_odom_.x; + double dy = new_odom.y - prev_odom_.y; + odom.x = cos(prev_odom_.a) * dx + sin(prev_odom_.a) * dy; + odom.y = -sin(prev_odom_.a) * dx + cos(prev_odom_.a) * dy; + odom.a = fmod(new_odom.a - prev_odom_.a + M_PI, 2 * M_PI) - M_PI; - if (!odom_set_) + prev_odom_ = new_odom; + return true; + } + + bool IO::resetOdometry() { - ROS_WARN("Odom was not yet set. It is set now."); + OdometryData new_odom; + if (!comm_->readOdometryData(new_odom)) + return false; prev_odom_ = new_odom; odom_set_ = true; - return false; + return true; } - odom.timestamp = new_odom.timestamp; //TODO give dt? - double dx = new_odom.x - prev_odom_.x; - double dy = new_odom.y - prev_odom_.y; - odom.x = cos(prev_odom_.a) * dx + sin(prev_odom_.a) * dy; - odom.y = -sin(prev_odom_.a) * dx + cos(prev_odom_.a) * dy; - odom.a = fmod(new_odom.a - prev_odom_.a + M_PI, 2*M_PI) - M_PI; + bool IO::readFrontBumperData(BumperData &bumper) + { + return comm_->readFrontBumperData(bumper); + } - prev_odom_ = new_odom; - return true; -} + bool IO::readBackBumperData(BumperData &bumper) + { + return comm_->readBackBumperData(bumper); + } -bool IO::resetOdometry() -{ - OdometryData new_odom; - if (!comm_->readOdometryData(new_odom)) - return false; - prev_odom_ = new_odom; - odom_set_ = true; - return true; -} + void IO::sendBaseReference(double vx, double vy, double va) + { + comm_->sendBaseVelocity(vx, vy, va); + } -bool IO::readFrontBumperData(BumperData& bumper) -{ - return comm_->readFrontBumperData(bumper); -} + void IO::sendOpendoorRequest() + { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" + system("aplay --device front:CARD=Device_1,DEV=0 ~/.emc/system/src/emc_system/sounds/doorbell.wav &"); +#pragma GCC diagnostic pop -bool IO::readBackBumperData(BumperData& bumper) -{ - return comm_->readBackBumperData(bumper); -} + comm_->sendOpenDoorRequest(); + } -void IO::sendBaseReference(double vx, double vy, double va) -{ - comm_->sendBaseVelocity(vx, vy, va); -} + void IO::speak(const std::string &text) + { + comm_->speak(text); + } -void IO::sendOpendoorRequest() -{ - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wunused-result" - system("aplay --device front:CARD=Device_1,DEV=0 ~/.emc/system/src/emc_system/sounds/doorbell.wav &"); - #pragma GCC diagnostic pop - - comm_->sendOpenDoorRequest(); -} + void IO::play(const std::string &file) + { + comm_->play(file); + } -void IO::speak(const std::string& text) -{ - comm_->speak(text); -} + bool IO::ok() + { + return rclcpp::ok(); + } -void IO::play(const std::string& file) -{ - comm_->play(file); -} + bool IO::sendPath(std::vector> path, std::array color, double width, int id) + { + return comm_->sendPath(path, color, width, id); + } -bool IO::ok() -{ - return rclcpp::ok(); -} + bool IO::sendPoseEstimate(double px, double py, double pz, double rx, double ry, double rz, double rw) + { + // apply map offset and send to comm_ + tf2::Transform pose; + pose.setOrigin(tf2::Vector3(px, py, pz)); + pose.setRotation(tf2::Quaternion(rx, ry, rz, rw)); -bool IO::sendPath(std::vector> path, std::array color, double width, int id) -{ - return comm_->sendPath(path, color, width, id); -} + comm_->sendPoseEstimate(tf2::toMsg(pose)); + return true; + } -bool IO::sendPoseEstimate(double px, double py, double pz, double rx, double ry, double rz, double rw) -{ - // apply map offset and send to comm_ - tf2::Transform pose; - pose.setOrigin(tf2::Vector3(px, py, pz)); - pose.setRotation(tf2::Quaternion(rx, ry, rz, rw)); + bool IO::sendPoseEstimate(double px, double py, double pz, double roll, double pitch, double yaw) + { + // convert roll pitch yaw to quaternion + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return this->sendPoseEstimate(px, py, pz, q.x(), q.y(), q.z(), q.w()); + } - comm_->sendPoseEstimate(tf2::toMsg(pose)); - return true; -} + bool IO::sendPoseEstimate(double x, double y, double yaw) + { + return this->sendPoseEstimate(x, y, 0, 0, 0, yaw); + } -bool IO::sendPoseEstimate(double px, double py, double pz, double roll, double pitch, double yaw) -{ - //convert roll pitch yaw to quaternion - tf2::Quaternion q; - q.setRPY(roll, pitch, yaw); - return this->sendPoseEstimate(px, py, pz, q.x(), q.y(), q.z(), q.w()); -} + // publishers used to visualize information in the localization exercises (particle filter): -bool IO::sendPoseEstimate(double x, double y, double yaw) -{ - return this->sendPoseEstimate(x, y, 0, 0, 0, yaw); -} + void IO::send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + { + comm_->send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); + } + + void IO::send_particles(int N, std::vector> particle_poses, double mapOrientation) + { + comm_->send_particles(N, particle_poses, mapOrientation); + } + + void IO::send_pose(std::vector pose, double mapOrientation) + { + comm_->send_pose(pose, mapOrientation); + } } From 141123ff34bd50844de00739af596592ac11c989 Mon Sep 17 00:00:00 2001 From: K de Vos Date: Fri, 12 Apr 2024 09:05:01 +0200 Subject: [PATCH 22/30] changed some function names to be consistent with the ROS1 branch (still regarding the visualization of the particle filter exercise) --- include/emc/communication.h | 6 +- include/emc/io.h | 6 +- include/emc/ros2publisher.h | 6 +- src/communication.cpp | 260 ++++++++++++++++++------------------ src/io.cpp | 12 +- 5 files changed, 144 insertions(+), 146 deletions(-) diff --git a/include/emc/communication.h b/include/emc/communication.h index e2df203..ff377a2 100644 --- a/include/emc/communication.h +++ b/include/emc/communication.h @@ -63,9 +63,9 @@ class Communication } // publishers used to visualize information in the localization exercises (particle filter): - void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); - void send_particles(int N, std::vector> particle_poses, double mapOrientation); - void send_pose(std::vector pose, double mapOrientation); + void localization_viz_send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); + void localization_viz_send_particles(int N, std::vector> particle_poses, double mapOrientation); + void localization_viz_send_pose(std::vector pose, double mapOrientation); private: Ros2Publisher* pub_node_; diff --git a/include/emc/io.h b/include/emc/io.h index ef8cc6b..4d1a801 100644 --- a/include/emc/io.h +++ b/include/emc/io.h @@ -129,9 +129,9 @@ class IO bool sendPoseEstimate(double x, double y, double yaw); // publishers used to visualize information in the localization exercises (particle filter): - void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); - void send_particles(int N, std::vector> particle_poses, double mapOrientation); - void send_pose(std::vector pose, double mapOrientation); + void localization_viz_send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction); + void localization_viz_send_particles(int N, std::vector> particle_poses, double mapOrientation); + void localization_viz_send_pose(std::vector pose, double mapOrientation); private: diff --git a/include/emc/ros2publisher.h b/include/emc/ros2publisher.h index 341b2db..089f5d1 100644 --- a/include/emc/ros2publisher.h +++ b/include/emc/ros2publisher.h @@ -147,7 +147,7 @@ namespace emc // publishers used to visualize information in the localization exercises (particle filter): - void send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + void localization_viz_send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) { sensor_msgs::msg::LaserScan msg{}; @@ -166,7 +166,7 @@ namespace emc this->pub_laser_msg->publish(msg); }; - void send_particles(int N, std::vector> particle_poses, double mapOrientation) + void localization_viz_send_particles(int N, std::vector> particle_poses, double mapOrientation) { geometry_msgs::msg::PoseArray msg{}; @@ -196,7 +196,7 @@ namespace emc }; - void send_pose(std::vector pose, double mapOrientation) + void localization_viz_send_pose(std::vector pose, double mapOrientation) { geometry_msgs::msg::PoseArray msg; diff --git a/src/communication.cpp b/src/communication.cpp index 7060d3a..9941e98 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -9,171 +9,169 @@ namespace emc { -Communication::Communication(std::string /*robot_name*/) -{ - std::cout << "constructor of Communication" << std::endl; - rclcpp::init(0,nullptr); - - std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; - // temp hardcode param names - laser_param = "scan"; - odom_param = "odometry/filtered"; - bumper_f_param = "bumper_f"; - bumper_b_param = "bumper_b"; - base_ref_param = "cmd_vel"; - open_door_param = "open_door"; - speak_param = "speak"; - play_param = "play"; - /* - // get robot parameters - if (!nh.getParam("laser_", laser_param)) {ROS_ERROR_STREAM("Parameter " << "laser_" << " not set");}; - if (!nh.getParam("odom_", odom_param)) {ROS_ERROR_STREAM("Parameter " << "odom_" << " not set");}; - if (!nh.getParam("bumper_f_", bumper_f_param)) {ROS_ERROR_STREAM("Parameter " << "bumper_f_" << " not set");}; - if (!nh.getParam("bumper_b_", bumper_b_param)) {ROS_ERROR_STREAM("Parameter " << "bumper_b_" << " not set");}; - if (!nh.getParam("base_ref_", base_ref_param)) {ROS_ERROR_STREAM("Parameter " << "base_ref_" << " not set");}; - if (!nh.getParam("open_door_", open_door_param)) {ROS_ERROR_STREAM("Parameter " << "open_door_" << " not set");}; - if (!nh.getParam("speak_", speak_param)) {ROS_ERROR_STREAM("Parameter " << "speak_" << " not set");}; - if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; - if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; -*/ - - laser_node_ = std::make_shared>(laser_param, "emc_laser"); - laser_executor_ = new rclcpp::executors::SingleThreadedExecutor; - laser_executor_->add_node(laser_node_); - - odom_node_ = std::make_shared>(odom_param, "emc_odom"); - odom_executor_ = new rclcpp::executors::SingleThreadedExecutor; - odom_executor_->add_node(odom_node_); - - pub_node_ = new Ros2Publisher(); -} - -Communication::~Communication() -{ - -} + Communication::Communication(std::string /*robot_name*/) + { + std::cout << "constructor of Communication" << std::endl; + rclcpp::init(0, nullptr); + + std::string laser_param, odom_param, bumper_f_param, bumper_b_param, base_ref_param, open_door_param, speak_param, play_param; + // temp hardcode param names + laser_param = "scan"; + odom_param = "odometry/filtered"; + bumper_f_param = "bumper_f"; + bumper_b_param = "bumper_b"; + base_ref_param = "cmd_vel"; + open_door_param = "open_door"; + speak_param = "speak"; + play_param = "play"; + /* + // get robot parameters + if (!nh.getParam("laser_", laser_param)) {ROS_ERROR_STREAM("Parameter " << "laser_" << " not set");}; + if (!nh.getParam("odom_", odom_param)) {ROS_ERROR_STREAM("Parameter " << "odom_" << " not set");}; + if (!nh.getParam("bumper_f_", bumper_f_param)) {ROS_ERROR_STREAM("Parameter " << "bumper_f_" << " not set");}; + if (!nh.getParam("bumper_b_", bumper_b_param)) {ROS_ERROR_STREAM("Parameter " << "bumper_b_" << " not set");}; + if (!nh.getParam("base_ref_", base_ref_param)) {ROS_ERROR_STREAM("Parameter " << "base_ref_" << " not set");}; + if (!nh.getParam("open_door_", open_door_param)) {ROS_ERROR_STREAM("Parameter " << "open_door_" << " not set");}; + if (!nh.getParam("speak_", speak_param)) {ROS_ERROR_STREAM("Parameter " << "speak_" << " not set");}; + if (!nh.getParam("play_", play_param)) {ROS_ERROR_STREAM("Parameter " << "play_" << " not set");}; + if (!nh.getParam("base_link_", robot_frame_name)) {ROS_ERROR_STREAM("Parameter " << "base_link_" << " not set");}; + */ -void Communication::init() -{ -} + laser_node_ = std::make_shared>(laser_param, "emc_laser"); + laser_executor_ = new rclcpp::executors::SingleThreadedExecutor; + laser_executor_->add_node(laser_node_); -bool Communication::readLaserData(LaserData& scan) -{ - laser_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. + odom_node_ = std::make_shared>(odom_param, "emc_odom"); + odom_executor_ = new rclcpp::executors::SingleThreadedExecutor; + odom_executor_->add_node(odom_node_); - sensor_msgs::msg::LaserScan msg; - if(!laser_node_->readMsg(msg)) - return false; + pub_node_ = new Ros2Publisher(); + } - scan.range_min = msg.range_min; - scan.range_max = msg.range_max; - scan.ranges = msg.ranges; - scan.angle_min = msg.angle_min; - scan.angle_max = msg.angle_max; - scan.angle_increment = msg.angle_increment; - scan.timestamp = rclcpp::Time(msg.header.stamp).seconds(); + Communication::~Communication() + { + } - return true; -} + void Communication::init() + { + } -bool Communication::readOdometryData(OdometryData& odom) -{ - odom_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. - - nav_msgs::msg::Odometry msg; - if(!odom_node_->readMsg(msg)) - return false; - - odom.x = msg.pose.pose.position.x; - odom.y = msg.pose.pose.position.y; + bool Communication::readLaserData(LaserData &scan) + { + laser_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. - // Calculate yaw rotation from quaternion - const geometry_msgs::msg::Quaternion& q = msg.pose.pose.orientation; - odom.a = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); + sensor_msgs::msg::LaserScan msg; + if (!laser_node_->readMsg(msg)) + return false; - odom.timestamp = rclcpp::Time(msg.header.stamp).seconds(); + scan.range_min = msg.range_min; + scan.range_max = msg.range_max; + scan.ranges = msg.ranges; + scan.angle_min = msg.angle_min; + scan.angle_max = msg.angle_max; + scan.angle_increment = msg.angle_increment; + scan.timestamp = rclcpp::Time(msg.header.stamp).seconds(); - return true; -} + return true; + } -bool Communication::readFrontBumperData(BumperData& /*bumper*/) -{ - return false; - /* - bumper_f_msg_.reset(); + bool Communication::readOdometryData(OdometryData &odom) + { + odom_executor_->spin_once(std::chrono::nanoseconds(0)); // wait 0 nanoseconds for new messages. just empty the buffer. + + nav_msgs::msg::Odometry msg; + if (!odom_node_->readMsg(msg)) + return false; + + odom.x = msg.pose.pose.position.x; + odom.y = msg.pose.pose.position.y; - //bumper_f_cb_queue_.callAvailable(); + // Calculate yaw rotation from quaternion + const geometry_msgs::msg::Quaternion &q = msg.pose.pose.orientation; + odom.a = atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)); - if (!bumper_f_msg_) + odom.timestamp = rclcpp::Time(msg.header.stamp).seconds(); + + return true; + } + + bool Communication::readFrontBumperData(BumperData & /*bumper*/) + { return false; + /* + bumper_f_msg_.reset(); - bumper.contact = bumper_f_msg_->data; - return true; - */ -} + //bumper_f_cb_queue_.callAvailable(); -bool Communication::readBackBumperData(BumperData& /*bumper*/) -{ - return false; - /* - bumper_b_msg_.reset(); + if (!bumper_f_msg_) + return false; - //bumper_b_cb_queue_.callAvailable(); + bumper.contact = bumper_f_msg_->data; + return true; + */ + } - if (!bumper_b_msg_) + bool Communication::readBackBumperData(BumperData & /*bumper*/) + { return false; + /* + bumper_b_msg_.reset(); - bumper.contact = bumper_b_msg_->data; - return true; - */ -} + //bumper_b_cb_queue_.callAvailable(); -void Communication::sendBaseVelocity(double vx, double vy, double va) -{ - pub_node_->sendBaseVelocity(vx,vy, va); -} + if (!bumper_b_msg_) + return false; -void Communication::sendOpenDoorRequest() -{ - pub_node_->sendOpenDoorRequest(); -} + bumper.contact = bumper_b_msg_->data; + return true; + */ + } -void Communication::sendMarker(visualization_msgs::msg::Marker marker) -{ - pub_node_->sendMarker(marker); -} + void Communication::sendBaseVelocity(double vx, double vy, double va) + { + pub_node_->sendBaseVelocity(vx, vy, va); + } -void Communication::speak(const std::string& text) -{ - pub_node_->speak(text); -} + void Communication::sendOpenDoorRequest() + { + pub_node_->sendOpenDoorRequest(); + } -void Communication::play(const std::string& file) -{ - pub_node_->play(file); -} + void Communication::sendMarker(visualization_msgs::msg::Marker marker) + { + pub_node_->sendMarker(marker); + } -void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform& pose) -{ - pub_node_->sendPoseEstimate(pose); -} + void Communication::speak(const std::string &text) + { + pub_node_->speak(text); + } + + void Communication::play(const std::string &file) + { + pub_node_->play(file); + } + + void Communication::sendPoseEstimate(const geometry_msgs::msg::Transform &pose) + { + pub_node_->sendPoseEstimate(pose); + } -// publishers used to visualize information in the localization exercises (particle filter): + // publishers used to visualize information in the localization exercises (particle filter): - void Communication::send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + void Communication::localization_viz_send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) { - pub_node_->send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); + pub_node_->localization_viz_send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); } - void Communication::send_particles(int N, std::vector> particle_poses, double mapOrientation) + void Communication::localization_viz_send_particles(int N, std::vector> particle_poses, double mapOrientation) { - pub_node_->send_particles(N, particle_poses, mapOrientation); + pub_node_->localization_viz_send_particles(N, particle_poses, mapOrientation); } - void Communication::send_pose(std::vector pose, double mapOrientation) + void Communication::localization_viz_send_pose(std::vector pose, double mapOrientation) { - pub_node_->send_pose(pose, mapOrientation); + pub_node_->localization_viz_send_pose(pose, mapOrientation); } } // end namespace emc - diff --git a/src/io.cpp b/src/io.cpp index 6611b46..039ab74 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -143,19 +143,19 @@ namespace emc // publishers used to visualize information in the localization exercises (particle filter): - void IO::send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) + void IO::localization_viz_send_laser_scan(double angle_min, double angle_max, double angle_inc, int subsample, std::vector prediction) { - comm_->send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); + comm_->localization_viz_send_laser_scan(angle_min, angle_max, angle_inc, subsample, prediction); } - void IO::send_particles(int N, std::vector> particle_poses, double mapOrientation) + void IO::localization_viz_send_particles(int N, std::vector> particle_poses, double mapOrientation) { - comm_->send_particles(N, particle_poses, mapOrientation); + comm_->localization_viz_send_particles(N, particle_poses, mapOrientation); } - void IO::send_pose(std::vector pose, double mapOrientation) + void IO::localization_viz_send_pose(std::vector pose, double mapOrientation) { - comm_->send_pose(pose, mapOrientation); + comm_->localization_viz_send_pose(pose, mapOrientation); } } From 0d0e35928bf8184ced609d184675460a14744baf Mon Sep 17 00:00:00 2001 From: Thijs Beurskens Date: Fri, 12 Apr 2024 13:55:10 +0200 Subject: [PATCH 23/30] export library --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b07f88f..9d9bb2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -add_library(emc_system +add_library(emc_system SHARED #include/emc/engine.h include/emc/communication.h include/emc/data.h @@ -95,7 +95,7 @@ target_link_libraries(emc_test_publisher emc_system) ############# ## Install ## ############# -install(TARGETS emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher +install(TARGETS emc_system emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher DESTINATION lib/${PROJECT_NAME} ) From 3c6bf56a933d40068e577a9b28fe7fb807789517 Mon Sep 17 00:00:00 2001 From: Thijs Beurskens Date: Fri, 12 Apr 2024 16:02:39 +0200 Subject: [PATCH 24/30] forward declare rclcpp rate --- include/emc/rate.h | 9 ++++----- src/rate.cpp | 2 ++ 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/emc/rate.h b/include/emc/rate.h index 01d1d53..6abb8e1 100644 --- a/include/emc/rate.h +++ b/include/emc/rate.h @@ -1,14 +1,13 @@ #ifndef EMC_SYSTEM_RATE_H_ #define EMC_SYSTEM_RATE_H_ -#include "rclcpp/rclcpp.hpp" -#include +#include -/*namespace rclcpp +namespace rclcpp { -class GenericRate; +template class GenericRate; } -*/ + namespace emc { diff --git a/src/rate.cpp b/src/rate.cpp index 117b984..d45c4e8 100644 --- a/src/rate.cpp +++ b/src/rate.cpp @@ -1,5 +1,7 @@ #include "emc/rate.h" +#include "rclcpp/rclcpp.hpp" +#include emc::Rate::Rate(double freq) { From b90ad1c741cbe2031bbfe5a0486a1770518d0046 Mon Sep 17 00:00:00 2001 From: Thijs Beurskens Date: Fri, 12 Apr 2024 16:15:26 +0200 Subject: [PATCH 25/30] change library name --- CMakeLists.txt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9d9bb2c..a842ea5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ include_directories( ${OpenCV_INCLUDE_DIRS} ) -add_library(emc_system SHARED +add_library(emc-framework SHARED #include/emc/engine.h include/emc/communication.h include/emc/data.h @@ -46,37 +46,37 @@ add_library(emc_system SHARED src/io.cpp src/rate.cpp ) -ament_target_dependencies(emc_system geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros) +ament_target_dependencies(emc-framework geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros) # ------------------------------------------------------------------------------------------------ # TOOLS # ------------------------------------------------------------------------------------------------ add_executable(emc_viz tools/visualize.cpp) -target_link_libraries(emc_viz emc_system ${OpenCV_LIBRARIES}) +target_link_libraries(emc_viz emc-framework ${OpenCV_LIBRARIES}) # ------------------------------------------------------------------------------------------------ # IO # ------------------------------------------------------------------------------------------------ add_executable(emc_test_io examples/test_io.cpp) -target_link_libraries(emc_test_io emc_system) +target_link_libraries(emc_test_io emc-framework) add_executable(emc_test_speech examples/testspeech.cpp) -target_link_libraries(emc_test_speech emc_system) +target_link_libraries(emc_test_speech emc-framework) # ------------------------------------------------------------------------------------------------ # EXAMPLES # ------------------------------------------------------------------------------------------------ add_executable(emc_example1 examples/example01.cpp) -target_link_libraries(emc_example1 emc_system) +target_link_libraries(emc_example1 emc-framework) add_executable(emc_test_subscriber examples/test_subscriber.cpp) -target_link_libraries(emc_test_subscriber emc_system) +target_link_libraries(emc_test_subscriber emc-framework) add_executable(emc_test_publisher examples/test_publisher.cpp) -target_link_libraries(emc_test_publisher emc_system) +target_link_libraries(emc_test_publisher emc-framework) #add_executable(emc_example2 examples/example02.cpp) #target_link_libraries(emc_example2 emc_system) @@ -95,7 +95,7 @@ target_link_libraries(emc_test_publisher emc_system) ############# ## Install ## ############# -install(TARGETS emc_system emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher +install(TARGETS emc-framework emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher DESTINATION lib/${PROJECT_NAME} ) From 04d1e6b1de6f0662f06a6fb6ffbf6c64328ccdbc Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 26 Nov 2024 15:01:24 +0100 Subject: [PATCH 26/30] add comments --- examples/test_io.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/test_io.cpp b/examples/test_io.cpp index 9d01051..3a12da8 100644 --- a/examples/test_io.cpp +++ b/examples/test_io.cpp @@ -5,12 +5,15 @@ int main() { + std::cout << "initialising" << std::endl; // Create IO object, which will initialize the io layer emc::IO io; + std::cout << "creating rate object" << std::endl; // Create Rate object, which will help using keeping the loop at a fixed frequency emc::Rate r(10); + std::cout << "starting loop" << std::endl; // Loop while we are properly connected while(io.ok()) { From c64ea07d5aa46c51826b4dc8fd128f62ddffc779 Mon Sep 17 00:00:00 2001 From: Peter van Dooren Date: Tue, 26 Nov 2024 15:02:22 +0100 Subject: [PATCH 27/30] add simple talker script to test subscribers --- scripts/talker.py | 48 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 scripts/talker.py diff --git a/scripts/talker.py b/scripts/talker.py new file mode 100644 index 0000000..965c692 --- /dev/null +++ b/scripts/talker.py @@ -0,0 +1,48 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String +from sensor_msgs.msg import LaserScan +from nav_msgs.msg import Odometry + + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('minimal_publisher') + self.odom_publisher_ = self.create_publisher(Odometry, 'odom', 10) + self.laser_publisher_ = self.create_publisher(LaserScan, 'laser_scan', 10) + + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + odom_msg = Odometry() + #odom_msg.data = 'Hello World: %d' % self.i + self.odom_publisher_.publish(odom_msg) + + laser_msg = LaserScan() + #odom_msg.data = 'Hello World: %d' % self.i + self.laser_publisher_.publish(laser_msg) + + self.get_logger().info('Publishing: "%i"' % self.i) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file From 32e9fad15626cab5fa5ecdbc63f48bab49c7fb84 Mon Sep 17 00:00:00 2001 From: Wiktor Date: Tue, 3 Dec 2024 16:19:30 +0100 Subject: [PATCH 28/30] compile and adjust libraries for ros2 --- .gitignore | 7 +++++++ CMakeLists.txt | 28 ++++++++++++++++------------ examples/example02.cpp | 1 + include/emc/data.h | 2 -- include/emc/rate.h | 15 ++------------- launch/hero_rviz.py | 13 +++++++++++++ scripts/talker.py | 2 +- src/communication.cpp | 1 - src/engine.cpp | 13 +++++++------ src/io.cpp | 2 +- src/rate.cpp | 2 -- 11 files changed, 48 insertions(+), 38 deletions(-) create mode 100644 launch/hero_rviz.py diff --git a/.gitignore b/.gitignore index cf25ac6..b5d0204 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,8 @@ CMakeLists.txt.user* +/build/ +/log/ +/install/ +.txt +*.txt +settings.json +*.pyc \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a842ea5..952eb73 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,9 @@ find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + + # find_package(Boost REQUIRED COMPONENTS system program_options) # find_package(PCL REQUIRED) @@ -27,10 +30,11 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} ) add_library(emc-framework SHARED - #include/emc/engine.h + include/emc/engine.h include/emc/communication.h include/emc/data.h include/emc/odom.h @@ -40,13 +44,13 @@ add_library(emc-framework SHARED include/emc/ros2subscriber.h include/emc/ros2publisher.h - #src/engine.cpp + src/engine.cpp src/communication.cpp #src/data.cpp src/io.cpp src/rate.cpp ) -ament_target_dependencies(emc-framework geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros) +ament_target_dependencies(emc-framework geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros tf2_geometry_msgs) # ------------------------------------------------------------------------------------------------ # TOOLS @@ -78,24 +82,24 @@ target_link_libraries(emc_test_subscriber emc-framework) add_executable(emc_test_publisher examples/test_publisher.cpp) target_link_libraries(emc_test_publisher emc-framework) -#add_executable(emc_example2 examples/example02.cpp) -#target_link_libraries(emc_example2 emc_system) +add_executable(emc_example2 examples/example02.cpp) +target_link_libraries(emc_example2 emc-framework) # ------------------------------------------------------------------------------------------------ # UNIT TESTS # ------------------------------------------------------------------------------------------------ -#if(BUILD_TESTING) -# find_package(rostest REQUIRED) -# add_rostest_gtest(tests_io test/io.test test/test_io.cpp) -# target_link_libraries(tests_io emc_system) -# ament_target_dependencies(emc_system geometry_msgs sensor_msgs nav_msgs roscpp tf2 tf2_ros rostest) -#endif() +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(tests_io test/test_io.cpp) + target_link_libraries(tests_io emc-framework) + ament_target_dependencies(tests_io geometry_msgs sensor_msgs nav_msgs rclcpp tf2 tf2_ros) +endif() ############# ## Install ## ############# -install(TARGETS emc-framework emc_example1 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher +install(TARGETS emc-framework emc_example1 emc_example2 emc_test_io emc_test_speech emc_test_subscriber emc_test_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/examples/example02.cpp b/examples/example02.cpp index 6efae2c..407be6b 100644 --- a/examples/example02.cpp +++ b/examples/example02.cpp @@ -1,4 +1,5 @@ #include +#include #include diff --git a/include/emc/data.h b/include/emc/data.h index 44afe30..202eadd 100644 --- a/include/emc/data.h +++ b/include/emc/data.h @@ -32,7 +32,6 @@ struct ControlEffort // ---------------------------------------------------------------------------------------------------- -/* class FSMInterface { @@ -52,7 +51,6 @@ class FSMInterface std::string event_; }; -*/ } // end namespace emc #endif diff --git a/include/emc/rate.h b/include/emc/rate.h index 6abb8e1..23f5c09 100644 --- a/include/emc/rate.h +++ b/include/emc/rate.h @@ -1,34 +1,23 @@ #ifndef EMC_SYSTEM_RATE_H_ #define EMC_SYSTEM_RATE_H_ -#include - -namespace rclcpp -{ -template class GenericRate; -} +#include namespace emc { class Rate { - public: - Rate(double freq); - ~Rate(); - void sleep(); private: - - rclcpp::GenericRate* rate_; + rclcpp::Rate* rate_; //rclcpp::Logger logger_; }; - } // end namespace emc #endif diff --git a/launch/hero_rviz.py b/launch/hero_rviz.py new file mode 100644 index 0000000..96d6391 --- /dev/null +++ b/launch/hero_rviz.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', '$(find emc_system)/launch/hero.rviz'], + output='screen' + ) + ]) diff --git a/scripts/talker.py b/scripts/talker.py index 965c692..fcf9171 100644 --- a/scripts/talker.py +++ b/scripts/talker.py @@ -10,7 +10,7 @@ class MinimalPublisher(Node): def __init__(self): super().__init__('minimal_publisher') - self.odom_publisher_ = self.create_publisher(Odometry, 'odom', 10) + self.odom_publisher_ = self.create_publisher(Odometry, 'odom/filtered', 10) self.laser_publisher_ = self.create_publisher(LaserScan, 'laser_scan', 10) timer_period = 0.1 # seconds diff --git a/src/communication.cpp b/src/communication.cpp index 9941e98..813f71c 100644 --- a/src/communication.cpp +++ b/src/communication.cpp @@ -71,7 +71,6 @@ namespace emc scan.angle_max = msg.angle_max; scan.angle_increment = msg.angle_increment; scan.timestamp = rclcpp::Time(msg.header.stamp).seconds(); - return true; } diff --git a/src/engine.cpp b/src/engine.cpp index e92021b..2dd7826 100644 --- a/src/engine.cpp +++ b/src/engine.cpp @@ -1,8 +1,7 @@ #include "emc/engine.h" #include "emc/communication.h" -#include -#include +#include namespace emc { @@ -49,15 +48,17 @@ void Engine::run() io_ = new IO; - ros::Rate r(loop_freq_); - while(ros::ok()) + auto node = rclcpp::Node::make_shared("engine_node"); + rclcpp::Rate rate(loop_freq_); + + while (rclcpp::ok()) { FSMInterface fsm; StateDetail& s = state_details[state_]; s.func(fsm, *io_, user_data_); - if (!ros::ok()) + if (!rclcpp::ok()) break; int event_id = getEvent(fsm.event().c_str()); @@ -79,7 +80,7 @@ void Engine::run() state_ = it->second; } - r.sleep(); + rate.sleep(); } } diff --git a/src/io.cpp b/src/io.cpp index 039ab74..4771642 100644 --- a/src/io.cpp +++ b/src/io.cpp @@ -5,7 +5,7 @@ #include "rclcpp/rclcpp.hpp" // for rclcpp::ok() #include -#include +#include #include diff --git a/src/rate.cpp b/src/rate.cpp index d45c4e8..d4a296f 100644 --- a/src/rate.cpp +++ b/src/rate.cpp @@ -1,11 +1,9 @@ #include "emc/rate.h" - #include "rclcpp/rclcpp.hpp" #include emc::Rate::Rate(double freq) { - //rclcpp::Time::init(); rate_ = new rclcpp::Rate(freq); //logger_ = rclcpp::get_logger("ratelogger"); } From 3863ae4e17f07b6026f21c63a3cace6d6d57147d Mon Sep 17 00:00:00 2001 From: Wiktor Date: Fri, 6 Dec 2024 11:41:08 +0100 Subject: [PATCH 29/30] Odometry topic and laser scan topic updated: odometry/filtered and scan --- scripts/talker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/talker.py b/scripts/talker.py index fcf9171..e019ed7 100644 --- a/scripts/talker.py +++ b/scripts/talker.py @@ -10,8 +10,8 @@ class MinimalPublisher(Node): def __init__(self): super().__init__('minimal_publisher') - self.odom_publisher_ = self.create_publisher(Odometry, 'odom/filtered', 10) - self.laser_publisher_ = self.create_publisher(LaserScan, 'laser_scan', 10) + self.odom_publisher_ = self.create_publisher(Odometry, 'odometry/filtered', 10) + self.laser_publisher_ = self.create_publisher(LaserScan, 'scan', 10) timer_period = 0.1 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) From a97335736978c40a7246c7c47beb685107c9a5ea Mon Sep 17 00:00:00 2001 From: Wiktor Date: Sun, 8 Dec 2024 20:45:49 +0100 Subject: [PATCH 30/30] Include library paths into the CMake file to avoid needing to set environment variable before launching executablees dependant on shaared library --- CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 952eb73..4762d36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,9 @@ add_library(emc-framework SHARED ) ament_target_dependencies(emc-framework geometry_msgs sensor_msgs visualization_msgs nav_msgs rclcpp tf2 tf2_ros tf2_geometry_msgs) +set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}") +set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE) + # ------------------------------------------------------------------------------------------------ # TOOLS # ------------------------------------------------------------------------------------------------