diff --git a/CMakeLists.txt b/CMakeLists.txt index 48d7af3..0708565 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,36 +9,17 @@ endif () # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(image_geometry REQUIRED) # Messages TODO_EXTRA -find_package(ackermann_msgs REQUIRED) +# find_package(ackermann_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -#find_package(std_msgs REQUIRED) # OpenCV TODO_EXTRA find_package(cv_bridge REQUIRED) find_package(OpenCV 4.2.0 REQUIRED) -#for ros listeners -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rclpy REQUIRED) -# define the publisher and subscriper nodes with the node names -add_executable(talker src/publisher_member_function.cpp) -ament_target_dependencies(talker rclcpp std_msgs) -install(TARGETS - talker - DESTINATION lib/${PROJECT_NAME}) - #define project name -#end ros listners includes - -# Add source for node executable (link non-ros dependencies here) -add_executable(polynomial_planner src/PolynomialPlanner.cpp src/PolynomialPlanner_node.cpp) -target_include_directories(polynomial_planner PUBLIC - $ - $) -target_compile_features(polynomial_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 # Node 2 add_executable(polynomial_planner_ai src/PolynomialPlannerAi.cpp src/PolynomialPlannerAi_node.cpp) @@ -51,7 +32,7 @@ target_compile_features(polynomial_planner_ai PUBLIC c_std_99 cxx_std_17) # Req set(dependencies rclcpp # Messages TODO_EXTRA - ackermann_msgs + # ackermann_msgs sensor_msgs std_msgs nav_msgs @@ -60,13 +41,12 @@ set(dependencies # OpenCv TODO_EXTRA cv_bridge OpenCV + # for camera space transformation + image_geometry ) # Link ros dependencies -ament_target_dependencies( - polynomial_planner - ${dependencies} -) + # Link ros dependencies ament_target_dependencies( @@ -74,8 +54,6 @@ ament_target_dependencies( ${dependencies} ) -install(TARGETS polynomial_planner - DESTINATION lib/${PROJECT_NAME}) install(TARGETS polynomial_planner_ai DESTINATION lib/${PROJECT_NAME}) @@ -97,7 +75,7 @@ if (BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}-test tests/unit.cpp # Remember to add node source files - src/PolynomialPlanner_node.cpp + src/PolynomialPlannerAi_node.cpp ) ament_target_dependencies(${PROJECT_NAME}-test ${dependencies}) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index f4ae2de..6de3afa 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -1,17 +1,41 @@ #pragma once +#include +#include +#include + +#include "image_geometry/pinhole_camera_model.h" +#include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" #include "std_msgs/msg/string.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" class PolynomialPlannerAi : public rclcpp::Node { private: - rclcpp::Publisher::SharedPtr pub; + rclcpp::Publisher::SharedPtr path_pub; + + rclcpp::Subscription::SharedPtr poly_sub; + + // Camera info sub & model vars + rclcpp::Subscription::SharedPtr rgb_info_sub; + image_geometry::PinholeCameraModel rgb_model; + + // std::optional + std::unique_ptr> Backend; - rclcpp::Subscription::SharedPtr sub; + // TF2 stuff + std::unique_ptr tf2_listener; + std::unique_ptr tf2_buffer; public: PolynomialPlannerAi(const rclcpp::NodeOptions& options); /// subscriber callback - void sub_cb(std_msgs::msg::String::SharedPtr msg); + /// 'PolynomialPlannerAi::' is unnesscary + void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, image_geometry::PinholeCameraModel camera_rgb); + void evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values); }; diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 686bc3e..7ece804 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,17 +1,39 @@ -#pragma once - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -class PolynomialPlanner : public rclcpp::Node { -private: - rclcpp::Publisher::SharedPtr pub; - - rclcpp::Subscription::SharedPtr sub; - -public: - PolynomialPlanner(const rclcpp::NodeOptions& options); - - /// subscriber callback - void sub_cb(std_msgs::msg::String::SharedPtr msg); -}; +//#pragma once +// +//#include +//#include +// +//#include "image_geometry/pinhole_camera_model.h" +//#include "nav_msgs/msg/path.hpp" +//#include "rclcpp/rclcpp.hpp" +//#include "sensor_msgs/msg/camera_info.hpp" +//#include "std_msgs/msg/string.hpp" +//#include "tf2_ros/buffer.h" +//#include "tf2_ros/transform_listener.h" +// +//class PolynomialPlanner : public rclcpp::Node { +//private: +// rclcpp::Publisher::SharedPtr path_pub; +// +// rclcpp::Subscription::SharedPtr sub; +// +// // Camera info sub & model vars +// rclcpp::Subscription::SharedPtr rgb_info_sub; +// image_geometry::PinholeCameraModel rgb_model; +// +// // std::optional +// std::unique_ptr> Backend; +// +// // TF2 stuff +// std::unique_ptr tf2_listener; +// std::unique_ptr tf2_buffer; +// +//public: +// PolynomialPlanner(const rclcpp::NodeOptions& options); +// +// /// subscriber callback +// void sub_cb(std_msgs::msg::String::SharedPtr msg); +// +// // camera transform +// nav_msgs::msg::Path cameraPixelToGroundPos(std::vector& pixels); +//}; diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 41306da..617bd2b 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -2,26 +2,30 @@ #include #include #include -#include +#include #include -#include "nav_msgs/msgs/Path.hpp" +#include + +#include "image_geometry/pinhole_camera_model.h" +#include "nav_msgs/msg/path.hpp" class Polynomial { public: // TODO what does std::pmr::vec mean compared to std::vec ??? - Polynomial(std::pmr::vector& vect) { this->vect = vect; } + Polynomial(std::vector& vect) { this->vect = vect; } Polynomial() = default; // store the vector private: - std::pmr::vector vect; + std::vector vect; public: // returns the y value at a given X. float poly(float x) { float result = 0; for (int i = 0; i < vect.size(); i++) { - int power = vect.size() - i; + int power = vect.size() - i - 1; // TODO this pow function might be wrong + // inproper way of accessing vect index result += vect[i] * pow(x, power); // a[n] * x ^ n } return result; @@ -29,14 +33,18 @@ class Polynomial { float polyDirvative(float x) { float result = 0; for (int i = 0; i < vect.size() - 1; i++) { - int power = vect.size() - i - 1; + int power = vect.size() - i - 2; // todo finish? - result += vect[i] * i * pow(x, power); // a[n] * n * x ^ (n - 1) + result += vect[i] * (power + 1) * pow(x, power); // a[n] * n * x ^ (n - 1) } + return result; } }; namespace backend { -std::optional create_path(const std::vector& leftPoly, - const std::vector& rightPoly std::string_view frame); -} \ No newline at end of file + +std::vector cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel rgb_info_sub); +std::optional create_path(std::vector& leftPoly, + image_geometry::PinholeCameraModel rgb_info_sub, std::string_view frame); +} // namespace backend diff --git a/package.xml b/package.xml index fbd9ed6..2b756ea 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ nav_msgs geometry_msgs tf2_geometry_msgs + image_geometry cv_bridge diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 4b28ec9..a4cf5f6 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,15 +1,89 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" +#include + +#include "backend.cpp" + // Required for doTransform -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "image_geometry/pinhole_camera_model.h" -// For _1 using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { + // RGB_INFO PARAMETER DO NOT DELETE + this->declare_parameter("camera_frame", "mid_cam_link"); + + this->rgb_info_sub = this->create_subscription( + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + + this->poly_sub = this->create_subscription( + "/road/polynomial", 1, [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) { + this->polynomial_cb(msg, this->rgb_model); + }); /// TODO fix paras + + this->path_pub = this->create_publisher("/path", 5); + RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data..."); + // TF2 things + this->tf2_buffer = std::make_unique(this->get_clock()); + this->tf2_listener = std::make_unique(*this->tf2_buffer); } -void PolynomialPlannerAi::sub_cb(std_msgs::msg::String::SharedPtr msg) { +void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, + image_geometry::PinholeCameraModel camera_rgb) { + // fix msg->empty + if (false) { + RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); + return; -} \ No newline at end of file + } else { + std::vector coeff{}; + int no_coeff = msg->data.size(); + + for (int i = 0; i < no_coeff; i++) { + coeff.push_back(msg->data[i]); + } + + + //std::string frame_id = this->get_parameter("camera_frame").as_string(); + //std::string frame_id = "notemptystring"; + // TODO camera frame_id is wrong + auto frame_id = this->get_parameter(std::string("camera_frame")).as_string(); + std::optional path_optional = backend::create_path(coeff, camera_rgb, frame_id); + nav_msgs::msg::Path path; + + + if (path_optional.has_value()) { + path = path_optional.value(); + std::string p = std::to_string(path.poses.size()); + RCLCPP_INFO(this->get_logger(), p.c_str()); + path.header.frame_id = this->get_parameter(std::string("camera_frame")).as_string(); + this->path_pub->publish(path); // error invalid operator *path + // Extract and print coefficients + // RCLCPP_INFO(this->get_logger(), "Received Polynomial Coefficients:"); + for (size_t i = 0; i < msg->data.size(); i++) { + // RCLCPP_INFO(this->get_logger(), "Coefficient[%zu] = %.15e", i, msg->data[i]); + } + } else { + std::string p = " error no path "; + RCLCPP_INFO(this->get_logger(), p.c_str()); + } + return; + } +} + +void PolynomialPlannerAi::evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values) { + for (float x : x_values) { + float y = 0.0; + for (size_t i = 0; i < coeffs.size(); i++) { + y += coeffs[i] * std::pow(x, i); + } + // RCLCPP_INFO(this->get_logger(), "P(%f) = %f", x, y); + } +} diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index e5ffd1a..a8f2111 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -1,15 +1,104 @@ #include "polynomial_planner/PolynomialPlanner_node.hpp" // Required for doTransform +#include + +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // For _1 using namespace std::placeholders; PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node("polynomial_planner", options) { + // delcare params + auto random_int = this->declare_parameter("random_int", 0); + this->declare_parameter("camera_frame", "mid_cam_link"); + + this->path_pub = this->create_publisher("/path", 5); + + // run once get pointer never used again + this->rgb_info_sub = this->create_subscription( + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + + // TF2 things + this->tf2_buffer = std::make_unique(this->get_clock()); + this->tf2_listener = std::make_unique(*this->tf2_buffer); +} + +void polynomial_cb(std_msgs::msg::String::SharedPtr) { + if (msg->empty()) { + RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); + return; + + } else { + std::vector coeff{}; + int no_coeff = msg->data.size(); + + for (int i = 0; i < no_coeff; i++) { + coeff.push_back(msg->data[i]); + } + // TODO fix params + nav_msgs::msg::Path path = backend::create_path(coeff, frame); + geometry_msgs::msg::PoseStamped path_poses[] = path.poses; + + bool do_logger = true; + if (do_logger) { + for (int i = 0; i < path_poses.size(); i++) { + RCLCPP_INFO(this->get_logger(), " we have polynomial"); + } + } + this->path_pub->publish(*path); + } } -void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) { +/* not so obvious reference: +geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vector& object_locations, + const cv::Mat& depth) { + geometry_msgs::msg::PoseArray poses{}; + poses.header.frame_id = this->get_parameter(std::string{"camera_frame"}).as_string(); + + // Rotation that rotates left 90 and backwards 90. + // This converts from camera coordinates in OpenCV to ROS coordinates + tf2::Quaternion optical_to_ros{}; + optical_to_ros.setRPY(-M_PI / 2, 0.0, -M_PI / 2); + + for (const cv::Point2d& center : object_locations) { + if (!this->rgb_model.initialized()) { + continue; + } + + // Project pixel to camera space + auto ray = this->rgb_model.projectPixelTo3dRay(center); + // The oak-d uses shorts in mm, sim uses f32 in m + float dist = depth.type() == 2 ? (float)depth.at(center) / 1000 : depth.at(center); + + // If depth unavailable, then skip + if (dist == INFINITY || dist >= 10 || dist <= 0) { + continue; + } -} \ No newline at end of file + // Just resize the ray to be a vector at the distance of the depth pixel. This is in camera space + auto point_3d = dist / cv::norm(ray) * ray; + + // Convert from camera space to ros coordinates ("World" but wrt camera mount) + tf2::Vector3 tf_vec{point_3d.x, point_3d.y, point_3d.z}; + auto world_vec = tf2::quatRotate(optical_to_ros, tf_vec); + + geometry_msgs::msg::Pose p{}; + p.position.x = world_vec.x(); + p.position.y = world_vec.y(); + p.position.z = world_vec.z(); + poses.poses.push_back(p); + } + + poses.header.stamp = this->get_clock()->now(); + return poses; +} + */ \ No newline at end of file diff --git a/src/backend.cpp b/src/backend.cpp index 44c4d06..ac987e5 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -1,111 +1,135 @@ #include "polynomial_planner/backend.hpp" -#include "geometry_msgs/msgs/PoseStamped.hpp" #include -std::optional backend::create_path(const std::vector& leftPoly, - const std::vector& rightPoly +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "image_geometry/pinhole_camera_model.h" + +std::optional backend::create_path(std::vector& leftPolyVector, + image_geometry::PinholeCameraModel rgb_info_sub, std::string_view frame) { -// std::string_view is a string lol - std::vector path; // this is the array for cone points - // this will make the - // Start path from kart - path.emplace_back(0, 0); + // std::string_view is a string lol + std::vector cam_path; // this is the vector of path plannign points + + int width = rgb_info_sub.fullResolution().width; // camera space sizes! + int height = rgb_info_sub.fullResolution().height; // Camera space sizes! + // take in Polynomial // ros polynoial take in code... // transfer to Polynomial class; - float is_right_valid = false; - float is_left_valid = false; + // float is_right_valid = false; + // float is_left_valid = false; - for (int i = 0; i < /* listenerArry.length*/; i++){ + auto leftPoly = new Polynomial(leftPolyVector); - is_left_valid = ( leftPoly == NULL) ? is_left_valid : ( leftPoly[i] != 0 ) ? true : is_left_valid; + // interval for polynomial + float max = height - height * .40; // artificial event horizon, + // the x value in which path points are no longer allowed to cross. + float interval = 3; // stepping x value up by 3camera px on each iteration + float start = height - height * 0.30; // bottom of frame + float threshold = 10.0; // min dist between points - is_right_valid = ( rightPoly == NULL) ? is_right_valid : ( rightPoly[i] != 0 ) ? true : is_right_valid; - } - leftPoly = (is_left_valid) ? new Polynomial( /* vector from ros listener */) : null; - if(is_left_valid){ - Polynomial leftPoly = new Polynomial( /* vector from ros listener */); - }else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; - } - rightPoly = (is_right_valid) ? new Polynomial( /* vector from ros listener */) : null; - if(is_right_valid){ - Polynomial rightPoly = new Polynomial( /* vector from ros listener */); - } else { - // TODO this is lazy and bad fix please - Polynomial rightPoly = null; - } + float dist = 0; // the value between the last published point and the current point + for (int x = start; x > max; x -= interval) { + dist += sqrt(interval * interval + pow(leftPoly->poly(x) - leftPoly->poly(x + interval), 2)); - // interval for polynomial - float max = 10; - float interval = 0.25; - float start = 0; - std::vector nav_points; - for (float i = start; i < max; i += interval){ - // generate points - nav_points.pushback(); - if (leftPoly != null){ - // do left poly math; - // - float dx = leftPoly.polyDirvative(x); - // calulate dx were dy is 1 - // becuase I dont want to use a wrapper class - float dy = 1; - // set dy to 1 - float l = std::sqrt( dx*dx + dy*dy); - // find the magnitude - dx = dx/l; // normalize - dy = dy/l; // normalize - cv::Point2d temp; - float x = i; // define x - float y = leftPoly.poly(x); // define y - temp.x = (x + 7 * dy); // project x - temp.y = (y - 7 * dx); // project y - nav_points[i].pushback(temp); - // return vector as < y, -x > - } - if (rightPoly != null){ - // do right poly math - // <-y, x> - // same steps different numbers - float dx = rightPoly.polyDirvative(x); - float dy = 1; - float l = std::sqrt( dx*dx + dy*dy); - l = std::hypot(dx, dy); - dx = dx/l; - dy = dy/l; - cv::Point2d temp; - float x = i; - float y = leftPoly.poly(x); - temp.x = (x - 7 * dy); - temp.y = (y + 7 * dx); - nav_points[i].pushback(temp); - // return vector as < -y , x > + if (dist > threshold) { + int translate = 480 - 224; + translate =0; + float camX = leftPoly->poly(x + translate); + float camY = x + translate; + // if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { + // CV point should be x,y + // the poly is P(y) + // translate should apply to the Y values? + cam_path.push_back(cv::Point2d(camX, camY)); + // } + dist = 0; } - // rememebr to return + } + for (int i = 55; i < 200; i++) { + float camX = 320; + float camY = (i) + 240; + if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { + // cam_path.push_back(cv::Point2d(camX, camY)); + } } - if (path.empty()) { + if (cam_path.empty()) { return std::nullopt; } else { - // Convert from cv types to ros - nav_msgs::msg::Path msg{}; - msg.header.frame_id = frame; + // Convert from cv types to nav::msg + // too few args + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); - // how do cv types differ from ros types. + nav_msgs::msg::Path msg{}; + // msg.header.frame_id = frame; + // for (cv::Point2d ground_points : ground_path) { + // std:: + // } // converting to message type in ROS - std::transform(path.begin(), path.end(), std::back_inserter(msg.poses), [&frame](const cv::Point2d& point) { - geometry_msgs::msg::PoseStamped pose{}; - pose.header.frame_id = frame; - pose.pose.position.x = point.x; - pose.pose.position.y = point.y; + std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), + [&frame](const cv::Point2d& point) { + geometry_msgs::msg::PoseStamped pose{}; + // frame = "redto0 isn't sure if we use this"; + // redto0 is SURE that we use this update and fix ASAP + pose.header.frame_id = frame; // literally is "notaemptystring" + pose.pose.position.x = point.x; + pose.pose.position.y = point.y; + // pose.pose.position.z = point.z; - return pose; - }); + return pose; + }); return msg; } } + +// why are the pointer things the way they are +// TODO: make it not die when z is too small +// or make z not too small +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel rgb_info_sub) { + // Rotation that rotates left 90 and backwards 90. + // This converts from camera coordinates in OpenCV to ROS coordinates + tf2::Quaternion optical_to_ros{}; + // set the Roll Pitch YAW + optical_to_ros.setRPY(0.0, 0.0, -M_PI / 2); + // optical_to_ros.setRPY(-M_PI / 2, 0.0, -M_PI / 2); + // optical_to_ros.setRPY(0.0, 0.0, 0.0); + + std::vector rwpoints; + + for (cv::Point2d& pixel : pixels) { + // gotta rectify the pixel before we raycast + // cv::Point2d rectPixel = rgb_info_sub.rectifyPoint(pixel); + cv::Point3d ray = rgb_info_sub.projectPixelTo3dRay(pixel); + + // -- CAMERA COORDINATES -- + // positive x = +X TO CAMERA + // positive y = STRAIGHT TO GROUND + // positive z = OUT OF CAMERA + // hopefully + + // ask zach for the trig, extend ray to the floor. + float divisor = ray.y / 0.6; + ray.x = ray.x / divisor; + ray.y = ray.y / divisor; + ray.z = ray.z / divisor; + // ray /= ray.z / -0.6; + // ray.z = (ray.z * -1); // we don't really care abt z, since it -will- *should* always just be cameraHeight + tf2::Vector3 tf_vec{ray.z, -ray.x, -ray.y}; + //tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; + //tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); + + //return type world_vec, use this is + + cv::Point2d dvector(tf_vec.x(), tf_vec.y()); + + // push back vectors + rwpoints.push_back(dvector); + } + + return rwpoints; +} diff --git a/src/publisher_member_function.cpp b/src/publisher_member_function.cpp deleted file mode 100644 index b211a5e..0000000 --- a/src/publisher_member_function.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -class MinimalPublisher : public rclcpp::Node -{ -public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } - -private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subscriber_member_function.cpp b/src/subscriber_member_function.cpp deleted file mode 100644 index 40f27f7..0000000 --- a/src/subscriber_member_function.cpp +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using std::placeholders::_1; - -class MinimalSubscriber : public rclcpp::Node -{ -public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } - -private: - void topic_callback(const std_msgs::msg::String & msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -}