From f7768114dbb6ecbb6d2419fc1d2dfee87cfe4581 Mon Sep 17 00:00:00 2001 From: Peter010103 Date: Wed, 12 Jan 2022 11:30:57 +0000 Subject: [PATCH] refactor: initial ROS2 refactoring refactor: ROS2 temporal provider examples include files --- freyja_examples/CMakeLists.txt | 55 +++---- freyja_examples/include/temporal_provider.h | 55 +++++++ freyja_examples/package.xml | 36 ++--- freyja_examples/src/temporal_provider.cpp | 168 +++++++++++--------- 4 files changed, 182 insertions(+), 132 deletions(-) create mode 100644 freyja_examples/include/temporal_provider.h diff --git a/freyja_examples/CMakeLists.txt b/freyja_examples/CMakeLists.txt index fbcbd1c..08ddbf6 100644 --- a/freyja_examples/CMakeLists.txt +++ b/freyja_examples/CMakeLists.txt @@ -1,40 +1,31 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(freyja_examples) -set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3" ) - -## 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 - message_generation - roscpp - freyja_msgs - std_msgs -) - +add_compile_options( -std=c++14 -O3 -ffast-math -Wall ) -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES trajectory_provider - CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp std_msgs -# DEPENDS system_lib -) +find_package(ament_cmake REQUIRED) -########### -## Build ## -########### +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(freyja_msgs REQUIRED) include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} + +add_executable(temporal_provider src/temporal_provider.cpp) + +target_include_directories( temporal_provider PUBLIC + $ + $ +) + +ament_target_dependencies( temporal_provider + rclcpp + freyja_msgs + std_msgs ) -# simple trajectory provider -add_executable(temporal_provider_node src/temporal_provider.cpp) -add_dependencies(temporal_provider_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +install(TARGETS + temporal_provider + DESTINATION lib/${PROJECT_NAME}) -target_link_libraries(temporal_provider_node - ${catkin_LIBRARIES} - ) - +ament_package() diff --git a/freyja_examples/include/temporal_provider.h b/freyja_examples/include/temporal_provider.h new file mode 100644 index 0000000..266b8a2 --- /dev/null +++ b/freyja_examples/include/temporal_provider.h @@ -0,0 +1,55 @@ +/* Provides a trajectory for the vehicle to follow. + EXAMPLE FILE; ONLY FOR SUPPORT. + Whatever you do here, output a time-based continuous function to follow. + This node should generate a 7 vector: [pn pe pd vn ve vd yaw]' for the vehicle + to follow. The controller currently listens to this reference trajectory + and updates its knowledge of the "latest" reference point. + + -- aj / 23rd Nov, 2017. +*/ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/u_int8.hpp" +#include "freyja_msgs/msg/reference_state.hpp" + +typedef freyja_msgs::msg::ReferenceState TrajRef; + +#define DEG2RAD(D) ((D)*3.1415326/180.0) + +using std::placeholders::_1; +using namespace std::chrono_literals; + +rclcpp::Time init_time; +std::string traj_type; +int agg_level; + +class Temporal_Traj : public rclcpp::Node +{ + TrajRef ref_state; + rclcpp::Time init_time; + + std::string traj_type; + int agg_level; + + public: + Temporal_Traj(); + + rclcpp::Subscription::SharedPtr time_reset_sub_; + void timer_reset_cb( const std_msgs::msg::UInt8::SharedPtr msg ); + + rclcpp::Publisher::SharedPtr traj_pub_; + + rclcpp::TimerBase::SharedPtr traj_update_timer_; + void traj_update_cb(); + + TrajRef getHoverReference( rclcpp::Duration cur_time ); + TrajRef getCircleReference( rclcpp::Duration cur_time, const int agg_level); + TrajRef getDefaultReference( rclcpp::Duration cur_time ); + TrajRef getLemiscateReference( rclcpp::Duration cur_time, const int agg_level); + +}; + diff --git a/freyja_examples/package.xml b/freyja_examples/package.xml index 5ea1e38..fc988cf 100644 --- a/freyja_examples/package.xml +++ b/freyja_examples/package.xml @@ -1,34 +1,20 @@ - + freyja_examples 0.0.0 - The examples package showing use cases - - - - - aj - + ROS2 Trajectory Examples + Peter Woo GPLv3 + ament_cmake - catkin - geometry_msgs - message_generation - roscpp - freyja_msgs - std_msgs - - geometry_msgs - message_runtime - roscpp - std_msgs - message_generation - - - - - + rclcpp + std_msgs + std_srvs + freyja_msgs + + ament_cmake + diff --git a/freyja_examples/src/temporal_provider.cpp b/freyja_examples/src/temporal_provider.cpp index 420c1bc..10745c5 100644 --- a/freyja_examples/src/temporal_provider.cpp +++ b/freyja_examples/src/temporal_provider.cpp @@ -9,30 +9,69 @@ -- aj / 23rd Nov, 2017. */ -#include -#include -#include -#include + +#include "temporal_provider.h" #define ROS_NODE_NAME "trajectory_provider" -typedef freyja_msgs::ReferenceState TrajRef; +#define UPDATE_RATE 50 // update rate of the trajectory (Hz) + +Temporal_Traj::Temporal_Traj() : Node( ROS_NODE_NAME ) +{ + init_time = now(); + + declare_parameter("traj_type", traj_type); + declare_parameter("agg_level", agg_level); -#define DEG2RAD(D) ((D)*3.1415326/180.0) + get_parameter("traj_type", traj_type); + get_parameter("agg_level", agg_level); -// global decl for "start time". This can be reset by a callback -ros::Time init_time; + time_reset_sub_ = create_subscription( + "/reset_trajectory_time", 1, std::bind(&Temporal_Traj::timer_reset_cb, this, _1)); + + traj_pub_ = create_publisher("/reference_state", 1); + + traj_update_timer_ = create_wall_timer( + 20ms, std::bind(&Temporal_Traj::traj_update_cb, this)); +} -void timeResetCallback( const std_msgs::UInt8::ConstPtr &msg ) +void Temporal_Traj::timer_reset_cb( std_msgs::msg::UInt8::SharedPtr msg ) { - if( msg -> data == 1 ) - { - init_time = ros::Time::now(); - ROS_WARN( "%s: Time reset requested!", ROS_NODE_NAME ); - } + if (msg->data == 1) + { + init_time = now(); + RCLCPP_WARN(get_logger(), "%s: Time reset requested!", ROS_NODE_NAME); + } } -// HOVER AT A POINT -TrajRef getHoverReference( const ros::Duration &cur_time ) +void Temporal_Traj::traj_update_cb() +{ + get_parameter("traj_type", traj_type); + + if (traj_type == "hover") + { + ref_state = Temporal_Traj::getHoverReference(now() - init_time); + RCLCPP_INFO(get_logger(), "Publishing Hover Reference States"); + } + else if (traj_type == "circle") + { + ref_state = Temporal_Traj::getCircleReference(now() - init_time, agg_level); + RCLCPP_INFO(get_logger(), "Publishing Circle Reference States"); + } + else if (traj_type == "lemiscate") + { + ref_state = Temporal_Traj::getLemiscateReference(now() - init_time, agg_level); + RCLCPP_INFO(get_logger(), "Publishing Lemiscate Reference States"); + } + else + { + RCLCPP_ERROR(get_logger(), "Invalid trajectory parameters given."); + } + + traj_pub_->publish(ref_state); +} + +// HOVER AT A POINT +TrajRef Temporal_Traj::getHoverReference( rclcpp::Duration cur_time ) { TrajRef ref_state; ref_state.pn = 0.0; @@ -45,19 +84,25 @@ TrajRef getHoverReference( const ros::Duration &cur_time ) ref_state.an = 0.0; ref_state.ae = 0.0; ref_state.ad = 0.0; - ref_state.header.stamp = ros::Time::now(); + ref_state.header.stamp = now(); return ref_state; } -// CIRCLE: pn = A*sin(wt), pe = A*cos(wt), vn = A*w*cos(wt) .. -TrajRef getCircleReference( const ros::Duration &cur_time, const int agg_level) +/** CIRCLE: +* pn = A*sin(wt) +* pe = A*cos(wt) +* vn = A*w*cos(wt) +* vn = -A*w*sin(wt) +*/ +TrajRef Temporal_Traj::getCircleReference( const rclcpp::Duration cur_time, const int agg_level) { // A is amplitude (radius); w angular rate such that 2pi/w = (seconds for one rev) float A = 0.5; float w = 0.5; // Set A and w based on agg_level - switch(agg_level) { + switch(agg_level) + { case 1 : break; case 2 : @@ -69,30 +114,31 @@ TrajRef getCircleReference( const ros::Duration &cur_time, const int agg_level) w = 3; break; default : - ROS_WARN_STREAM("Circle aggression " << agg_level << " not supported, defaulting to agg_level 1"); - } + RCLCPP_WARN(get_logger(), "Circle aggression %d not supported, defaulting to agg_level 1", agg_level); + } - float t = cur_time.toSec(); + float t = cur_time.seconds(); - // Create reference state - TrajRef ref_state; - ref_state.header.stamp = ros::Time::now(); + // Create reference state + TrajRef ref_state; + ref_state.header.stamp = now(); - ref_state.pn = A*std::sin( w*t ); - ref_state.pe = A*std::cos( w*t ); - ref_state.pd = -1.0; - - ref_state.vn = A*w*std::cos( w*t ); - ref_state.ve = -A*w*std::sin( w*t ); - ref_state.vd = 0.0; + ref_state.pn = A*std::sin( w*t ); + ref_state.pe = A*std::cos( w*t ); + ref_state.pd = -4.0; - ref_state.yaw = 0.0; + ref_state.vn = A*w*std::cos( w*t ); + ref_state.ve = -A*w*std::sin( w*t ); + ref_state.vd = 0.0; + + ref_state.yaw = 0.0; + + // set an, ae, ad to second derivatives if needed for FF.. + return ref_state; - // set an, ae, ad to second derivatives if needed for FF.. - return ref_state; } -TrajRef getDefaultReference( const ros::Duration &cur_time ) +TrajRef Temporal_Traj::getDefaultReference( rclcpp::Duration cur_time ) { TrajRef ref_state; ref_state.pn = 0.5; @@ -105,48 +151,20 @@ TrajRef getDefaultReference( const ros::Duration &cur_time ) ref_state.an = 0.0; ref_state.ae = 0.0; ref_state.ad = 0.0; - ref_state.header.stamp = ros::Time::now(); - return ref_state; + ref_state.header.stamp = now(); + + return ref_state; } -int main( int argc, char** argv ) { - ros::init( argc, argv, ROS_NODE_NAME ); - ros::NodeHandle nh, priv_nh("~"); - /* Publisher for trajectory */ - ros::Publisher traj_pub; - traj_pub = nh.advertise ( "/reference_state", 1, true ); - - /* Create subscriber for resetting time -- restart the trajectory */ - ros::Subscriber time_reset_sub; - time_reset_sub = nh.subscribe( "/reset_trajectory_time", 1, timeResetCallback ); - - std::string traj_type; - priv_nh.param( "example_traj_type", traj_type, std::string("hover") ); - - /* How fast should a trajectory update be made? */ - ros::Rate update_rate(50); - init_time = ros::Time::now(); - - while( ros::ok() ) - { - TrajRef ref_state; - if( traj_type == "circle1" ) - ref_state = getCircleReference( ros::Time::now() - init_time, 1 ); - else if( traj_type == "circle2" ) - ref_state = getCircleReference( ros::Time::now() - init_time, 2 ); - else if( traj_type == "circle3" ) - ref_state = getCircleReference( ros::Time::now() - init_time, 3 ); - else if( traj_type == "hover" ) - ref_state = getHoverReference( ros::Time::now() - init_time ); - else - ref_state = getDefaultReference( ros::Time::now() - init_time ); - traj_pub.publish( ref_state ); - - ros::spinOnce(); - update_rate.sleep(); - } + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } +