From: Walter Fetter Lages Date: Thu, 6 Jul 2023 07:18:09 +0000 (-0300) Subject: Port to Galactic. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=87600a317037196139ca2984f412a33a742e471c;p=pose2d_trajectories.git Port to Galactic. --- diff --git a/.gitignore b/.gitignore index 35d74bb..229a5a3 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -devel/ +install/ logs/ build/ bin/ @@ -49,3 +49,6 @@ qtcreator-* # Catkin custom files CATKIN_IGNORE + +# Colcom custom files +COLCON_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a4829b..8632316 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,221 +1,82 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(pose2d_trajectories) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Eigen3 REQUIRED) - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# geometry_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES pose2d_trajectories -# CATKIN_DEPENDS geometry_msgs roscpp - DEPENDS EIGEN3 +add_library(pose2d_trajectories + src/circle_path.cpp + src/eight_path.cpp) +target_compile_features(pose2d_trajectories PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(pose2d_trajectories PUBLIC + $ + $ + $) +ament_target_dependencies( + pose2d_trajectories + "rclcpp" + "geometry_msgs" + "Eigen3" ) -########### -## Build ## -########### +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pose2d_trajectories PRIVATE "POSE2D_TRAJECTORIES_BUILDING_LIBRARY") -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -# TODO: Check names of system library include directories (Eigen3) - ${EIGEN3_INCLUDE_DIRS} +install( + DIRECTORY include/ + DESTINATION include ) - -## Declare a C++ library -add_library(${PROJECT_NAME} - src/circle_path.cpp - src/eight_path.cpp +install( + TARGETS pose2d_trajectories + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide add_executable(eight_trajectory src/eight_trajectory.cpp) -add_executable(pose2d_stamp src/pose2d_stamp.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +target_include_directories(eight_trajectory PUBLIC + $ + $) +target_link_libraries(eight_trajectory pose2d_trajectories) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -# ${Eigen3_LIBRARIES} -) - -target_link_libraries(eight_trajectory - ${catkin_LIBRARIES} - ${PROJECT_NAME} -) - -target_link_libraries(pose2d_stamp - ${catkin_LIBRARIES} -) - - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +add_executable(pose2d_stamp src/pose2d_stamp.cpp) +target_include_directories(pose2d_stamp PUBLIC + $ + $) +target_link_libraries(pose2d_stamp pose2d_trajectories) -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html install(TARGETS eight_trajectory pose2d_stamp - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +ament_export_libraries( + pose2d_trajectories ) - -## Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE +ament_export_targets( + export_${PROJECT_NAME} ) -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_pose2d_trajectories.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/include/pose2d_trajectories/circle_path.h b/include/pose2d_trajectories/circle_path.h deleted file mode 100644 index 755939f..0000000 --- a/include/pose2d_trajectories/circle_path.h +++ /dev/null @@ -1,77 +0,0 @@ -/****************************************************************************** - ROS pose2d_trajectories Package - Circle Path Class - Copyright (C) 2015..2018 Walter Fetter Lages - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see - . - -*******************************************************************************/ - -#ifndef CIRCLE_PATH_H -#define CIRCLE_PATH_H - -#include - -/** Circle path -* @author Walter Fetter Lages -*/ -class CirclePath -{ - Eigen::Vector2d pc_; - double phi0_; - double r_; - double w_; - - public: - - /** Build a circle path. - * @param pc circle center point. - * @param phi0 initial phase. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); - - /** Build a circle path. - * @param p0 starting pose. - * @param r circle radius. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector3d &p0,double r,double w); - - /** Build a circle path. - * @param pc circle center point. - * @param p0 starting point. - * @param w angular velocity. - */ - CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); - - /** Destroy a circle path. - */ - ~CirclePath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/include/pose2d_trajectories/circle_path.hpp b/include/pose2d_trajectories/circle_path.hpp new file mode 100644 index 0000000..e475798 --- /dev/null +++ b/include/pose2d_trajectories/circle_path.hpp @@ -0,0 +1,89 @@ +/****************************************************************************** + ROS 2 pose2d_trajectories Package + Circle Path Class + Copyright (C) 2015..2022 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef POSE2D_TRAJECTORIES__CIRCLE_PATH_HPP_ +#define POSE2D_TRAJECTORIES__CIRCLE_PATH_HPP_ + +#include + +#include + +namespace pose2d_trajectories +{ + + /** Circle path + * @author Walter Fetter Lages + */ + class CirclePath + { + Eigen::Vector2d pc_; + double phi0_; + double r_; + double w_; + + public: + + /** Build a circle path. + * @param pc circle center point. + * @param phi0 initial phase. + * @param r circle radius. + * @param w angular velocity. + */ + POSE2D_TRAJECTORIES_PUBLIC + CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); + + /** Build a circle path. + * @param p0 starting pose. + * @param r circle radius. + * @param w angular velocity. + */ + POSE2D_TRAJECTORIES_PUBLIC + CirclePath(const Eigen::Vector3d &p0,double r,double w); + + /** Build a circle path. + * @param pc circle center point. + * @param p0 starting point. + * @param w angular velocity. + */ + POSE2D_TRAJECTORIES_PUBLIC + CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); + + /** Destroy a circle path. + */ + POSE2D_TRAJECTORIES_PUBLIC + ~CirclePath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + POSE2D_TRAJECTORIES_PUBLIC + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + POSE2D_TRAJECTORIES_PUBLIC + Eigen::Vector2d steering(double t) const; + }; +} +#endif diff --git a/include/pose2d_trajectories/eight_path.h b/include/pose2d_trajectories/eight_path.h deleted file mode 100644 index 401a331..0000000 --- a/include/pose2d_trajectories/eight_path.h +++ /dev/null @@ -1,61 +0,0 @@ -/****************************************************************************** - ROS pose2d_trajectories Package - Eigth Path Class - Copyright (C) 2015..2018 Walter Fetter Lages - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see - . - -*******************************************************************************/ - -#ifndef EIGHT_PATH_H -#define EIGHT_PATH_H - -#include - -/** 8 path -* @author Walter Fetter Lages -*/ -class EightPath -{ - CirclePath c1_; - CirclePath c2_; - double period_; - - public: - - /** Build an 8 path. - * @param pc center point. - * @param r radius. - * @param w angular velocity. - */ - EightPath(const Eigen::Vector2d &pc,double r,double w); - - /** Destroy an 8 path. - */ - ~EightPath(void) { }; - - /** Get path point. - * @param t path time. - * @return path point. - */ - Eigen::Vector3d point(double t) const; - - /** Get path steering controls. - * @param t path time. - * @return steering controls. - */ - Eigen::Vector2d steering(double t) const; -}; -#endif diff --git a/include/pose2d_trajectories/eight_path.hpp b/include/pose2d_trajectories/eight_path.hpp new file mode 100644 index 0000000..3f95ed4 --- /dev/null +++ b/include/pose2d_trajectories/eight_path.hpp @@ -0,0 +1,70 @@ +/****************************************************************************** + ROS2 pose2d_trajectories Package + Eigth Path Class + Copyright (C) 2015..2022 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef POSE2D_TRAJECTORIES__EIGHT_PATH_HPP_ +#define POSE2D_TRAJECTORIES__EIGHT_PATH_HPP_ + +#include + +#include + +namespace pose2d_trajectories +{ + /** 8 path + * @author Walter Fetter Lages + */ + class EightPath + { + CirclePath c1_; + CirclePath c2_; + double period_; + + public: + + /** Build an 8 path. + * @param pc center point. + * @param r radius. + * @param w angular velocity. + */ + POSE2D_TRAJECTORIES_PUBLIC + EightPath(const Eigen::Vector2d &pc,double r,double w); + + /** Destroy an 8 path. + */ + POSE2D_TRAJECTORIES_PUBLIC + ~EightPath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + POSE2D_TRAJECTORIES_PUBLIC + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + POSE2D_TRAJECTORIES_PUBLIC + Eigen::Vector2d steering(double t) const; + }; +} +#endif diff --git a/include/pose2d_trajectories/visibility_control.h b/include/pose2d_trajectories/visibility_control.h new file mode 100644 index 0000000..ec97ff9 --- /dev/null +++ b/include/pose2d_trajectories/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef POSE2D_TRAJECTORIES__VISIBILITY_CONTROL_H_ +#define POSE2D_TRAJECTORIES__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define POSE2D_TRAJECTORIES_EXPORT __attribute__ ((dllexport)) + #define POSE2D_TRAJECTORIES_IMPORT __attribute__ ((dllimport)) + #else + #define POSE2D_TRAJECTORIES_EXPORT __declspec(dllexport) + #define POSE2D_TRAJECTORIES_IMPORT __declspec(dllimport) + #endif + #ifdef POSE2D_TRAJECTORIES_BUILDING_LIBRARY + #define POSE2D_TRAJECTORIES_PUBLIC POSE2D_TRAJECTORIES_EXPORT + #else + #define POSE2D_TRAJECTORIES_PUBLIC POSE2D_TRAJECTORIES_IMPORT + #endif + #define POSE2D_TRAJECTORIES_PUBLIC_TYPE POSE2D_TRAJECTORIES_PUBLIC + #define POSE2D_TRAJECTORIES_LOCAL +#else + #define POSE2D_TRAJECTORIES_EXPORT __attribute__ ((visibility("default"))) + #define POSE2D_TRAJECTORIES_IMPORT + #if __GNUC__ >= 4 + #define POSE2D_TRAJECTORIES_PUBLIC __attribute__ ((visibility("default"))) + #define POSE2D_TRAJECTORIES_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define POSE2D_TRAJECTORIES_PUBLIC + #define POSE2D_TRAJECTORIES_LOCAL + #endif + #define POSE2D_TRAJECTORIES_PUBLIC_TYPE +#endif + +#endif // POSE2D_TRAJECTORIES__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index bb35cb3..e7da593 100644 --- a/package.xml +++ b/package.xml @@ -1,69 +1,22 @@ - + + pose2d_trajectories - 3.2.0 - The pose2d_trajectories package - - - - + 4.0.0 + Pose 2D trajectory generation Walter Fetter Lages - - - - - GPLv3 + ament_cmake_ros - - - - - + rclcpp + geometry_msgs + Eigen3 - - - - - Walter Fetter Lages + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - Eigen3 - geometry_msgs - roscpp - Eigen3 - geometry_msgs - roscpp - Eigen3 - geometry_msgs - roscpp - - - - - + ament_cmake diff --git a/src/circle_path.cpp b/src/circle_path.cpp index df5f920..469301a 100644 --- a/src/circle_path.cpp +++ b/src/circle_path.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS pose2d_trajectories Package + ROS 2 pose2d_trajectories Package Circle Path Class - Copyright (C) 2015..2018 Walter Fetter Lages + Copyright (C) 2015..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,11 +19,14 @@ *******************************************************************************/ -#include +#include #define sqr(x) ((x)*(x)) #define sgn(x) (((x) == 0.0)? 0.0:((x)/fabs(x))) +namespace pose2d_trajectories +{ + CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) { pc_=pc; @@ -61,7 +64,7 @@ Eigen::Vector3d CirclePath::point(double t) const return p; } -Eigen::Vector2d CirclePath::steering(double t) const +Eigen::Vector2d CirclePath::steering(double /* t */) const { Eigen::Vector2d e(2); @@ -70,3 +73,5 @@ Eigen::Vector2d CirclePath::steering(double t) const return e; } + +} diff --git a/src/eight_path.cpp b/src/eight_path.cpp index 02326e4..6307c0e 100644 --- a/src/eight_path.cpp +++ b/src/eight_path.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS pose2d_trajectories Package + ROS 2 pose2d_trajectories Package Eigth Path Class - Copyright (C) 2015..2018 Walter Fetter Lages + Copyright (C) 2015..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,16 +19,19 @@ *******************************************************************************/ -#include +#include #define sgn(x) (((x) == 0.0)? 0.0:((x)/fabs(x))) +namespace pose2d_trajectories +{ + EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) { period_=4*M_PI/w; -}; +} Eigen::Vector3d EightPath::point(double t) const { @@ -45,3 +48,5 @@ Eigen::Vector2d EightPath::steering(double t) const if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); else return c2_.steering(tc); } + +} diff --git a/src/eight_trajectory.cpp b/src/eight_trajectory.cpp index b521aa1..da6d26f 100644 --- a/src/eight_trajectory.cpp +++ b/src/eight_trajectory.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS pose2d_trajectories Package + ROS 2 pose2d_trajectories Package Eigth Trajectory Node - Copyright (C) 2015..2018 Walter Fetter Lages + Copyright (C) 2015..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,76 +19,79 @@ *******************************************************************************/ -#include -#include +#include +#include -#include +#include -class EightTrajectory +class EightTrajectory: public rclcpp::Node { public: - EightTrajectory(ros::NodeHandle node); - ~EightTrajectory(void); - void setCommand(ros::Duration t); + EightTrajectory(void); + void setCommandCB(void); private: - ros::NodeHandle node_; - const EightPath *path; + std::unique_ptr path_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time t0_; - ros::Publisher commandPublisher; + rclcpp::Publisher::SharedPtr commandPublisher_; }; -EightTrajectory::EightTrajectory(ros::NodeHandle node) +EightTrajectory::EightTrajectory(void): Node("eight_trajectory") { - node_=node; Eigen::Vector2d pc; double w; double r; - ros::param::get("~x",pc[0]); - ros::param::get("~y",pc[1]); - ros::param::get("~radius",r); - ros::param::get("~ang_vel",w); - - path=new EightPath(pc,r,w); - commandPublisher=node_.advertise("command",1000); -} + try + { + declare_parameter("x",rclcpp::PARAMETER_DOUBLE); + if(!get_parameter("x",pc[0])) + throw std::runtime_error("No 'x' parameter."); + + declare_parameter("y",rclcpp::PARAMETER_DOUBLE); + if(!get_parameter("y",pc[1])) + throw std::runtime_error("No 'y' parameter."); -EightTrajectory::~EightTrajectory(void) -{ - commandPublisher.shutdown(); - delete path; + declare_parameter("radius",rclcpp::PARAMETER_DOUBLE); + if(!get_parameter("radius",r)) + throw std::runtime_error("No 'radius' parameter."); + + declare_parameter("ang_vel",rclcpp::PARAMETER_DOUBLE); + if(!get_parameter("ang_vel",w)) + throw std::runtime_error("No 'ang_vel' parameter."); + } + catch(const std::exception &e) + { + RCLCPP_ERROR(get_logger(), e.what()); + return; + } + + path_=std::make_unique(pc,r,w); + commandPublisher_=create_publisher("command",100); + + timer_=rclcpp::create_timer(this,get_clock(),rclcpp::Duration::from_seconds(0.01),std::bind(&EightTrajectory::setCommandCB,this)); + t0_=get_clock()->now(); } -void EightTrajectory::setCommand(ros::Duration t) +void EightTrajectory::setCommandCB(void) { - Eigen::Vector3d p=path->point(t.toSec()); + rclcpp::Duration dt=get_clock()->now()-t0_; + Eigen::Vector3d p=path_->point(dt.seconds()); - geometry_msgs::Pose2D command; + geometry_msgs::msg::Pose2D command; command.x=p[0]; command.y=p[1]; command.theta=p[2]; - commandPublisher.publish(command); + commandPublisher_->publish(command); } int main(int argc,char* argv[]) { - - ros::init(argc,argv,"eight_trajectory"); - ros::NodeHandle node; - - EightTrajectory eightTrajectory(node); - - ros::Rate loop(100); - - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - eightTrajectory.setCommand(ros::Time::now()-t0); - - ros::spinOnce(); - loop.sleep(); - } + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/src/pose2d_stamp.cpp b/src/pose2d_stamp.cpp index ae0dcc5..9e6976f 100644 --- a/src/pose2d_stamp.cpp +++ b/src/pose2d_stamp.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS pose2d_trajectories Package + ROS 2 pose2d_trajectories Package Pose 2D Stamp Node - Copyright (C) 2015..2018 Walter Fetter Lages + Copyright (C) 2015..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,55 +19,45 @@ *******************************************************************************/ -#include +#include #include -#include -#include +#include +#include -class Pose2DStamp +class Pose2DStamp: public rclcpp::Node { public: - Pose2DStamp(ros::NodeHandle node); - ~Pose2DStamp(void); + Pose2DStamp(void); private: - ros::NodeHandle node_; - - ros::Subscriber poseSubscriber; - ros::Publisher posePublisher; + rclcpp::Subscription::SharedPtr poseSubscriber_; + rclcpp::Publisher::SharedPtr posePublisher_; - int seq; - std::string frame_id; + std::string frame_id_; - void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); + void poseCB(const geometry_msgs::msg::Pose2D::SharedPtr pose); }; -Pose2DStamp::Pose2DStamp(ros::NodeHandle node) +Pose2DStamp::Pose2DStamp(void): Node("pose2d_stamp") { - node_=node; - - poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); - posePublisher=node_.advertise("command_stamped",1000); - - seq=0; - - ros::param::get("~frame_id",frame_id); -} + declare_parameter("frame_id",rclcpp::PARAMETER_STRING); + if(!get_parameter("frame_id",frame_id_)) + RCLCPP_WARN(get_logger(),"No 'frame_id' parameter."); -Pose2DStamp::~Pose2DStamp(void) -{ - poseSubscriber.shutdown(); - posePublisher.shutdown(); + posePublisher_=create_publisher("command_stamped",100); + + using std::placeholders::_1; + poseSubscriber_=create_subscription("command",100,std::bind(&Pose2DStamp::poseCB,this,_1)); } -void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) +void Pose2DStamp::poseCB(const geometry_msgs::msg::Pose2D::SharedPtr pose) { - geometry_msgs::PoseStamped stamped; - stamped.header.stamp=ros::Time::now(); - stamped.header.frame_id=frame_id; + geometry_msgs::msg::PoseStamped stamped; + stamped.header.stamp=get_clock()->now(); + stamped.header.frame_id=frame_id_; stamped.pose.position.x=pose->x; stamped.pose.position.y=pose->y; stamped.pose.position.z=0; @@ -75,22 +65,13 @@ void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) stamped.pose.orientation.y=0; stamped.pose.orientation.z=sin(pose->theta/2.0); stamped.pose.orientation.w=cos(pose->theta/2.0); - posePublisher.publish(stamped); + posePublisher_->publish(stamped); } int main(int argc,char* argv[]) { - ros::init(argc,argv,"pose2d_stamp"); - ros::NodeHandle node; - - Pose2DStamp pose2DStamp(node); - - ros::Rate loop(100); - ros::Time t0=ros::Time::now(); - while(ros::ok()) - { - ros::spinOnce(); - loop.sleep(); - } + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; }