Skip to content

Commit

Permalink
refactor: initial ROS2 refactoring
Browse files Browse the repository at this point in the history 
refactor: ROS2 temporal provider examples include files
  • Loading branch information
@Peter010103
Peter010103 committed Jan 12, 2022
1 parent 9d65d33 commit f776811
Show file tree
Hide file tree
Showing 4 changed files with 182 additions and 132 deletions.
55 changes: 23 additions & 32 deletions freyja_examples/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)

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()
55 changes: 55 additions & 0 deletions freyja_examples/include/temporal_provider.h
View file
Original file line number Diff line number Diff line change
@@ -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 <chrono>
#include <cmath>
#include <memory>

#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<std_msgs::msg::UInt8>::SharedPtr time_reset_sub_;
void timer_reset_cb( const std_msgs::msg::UInt8::SharedPtr msg );

rclcpp::Publisher<TrajRef>::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);

};

36 changes: 11 additions & 25 deletions freyja_examples/package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,34 +1,20 @@
<?xml version="1.0"?>
<package>
<package format="3">
<name>freyja_examples</name>
<version>0.0.0</version>
<description>The examples package showing use cases</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
<maintainer email="[email protected]">aj</maintainer>

<description>ROS2 Trajectory Examples</description>
<maintainer email="[email protected]">Peter Woo</maintainer>
<license>GPLv3</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>freyja_msgs</build_depend>
<build_depend>std_msgs</build_depend>

<run_depend>geometry_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_generation</run_depend>


<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>freyja_msgs</depend>

<export>
<build_type>ament_cmake</build_type>
</export>

</package>
168 changes: 93 additions & 75 deletions freyja_examples/src/temporal_provider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,30 +9,69 @@
-- aj / 23rd Nov, 2017.
*/
#include <cmath>
#include <ros/ros.h>
#include <std_msgs/UInt8.h>
#include <freyja_msgs/ReferenceState.h>

#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<std::string>("traj_type", traj_type);
declare_parameter<int>("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<std_msgs::msg::UInt8>(
"/reset_trajectory_time", 1, std::bind(&Temporal_Traj::timer_reset_cb, this, _1));

traj_pub_ = create_publisher<TrajRef>("/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;
Expand All @@ -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 :
Expand All @@ -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;
Expand All @@ -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 <TrajRef> ( "/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<Temporal_Traj>());
rclcpp::shutdown();
return 0;
}

0 comments on commit f776811

Please sign in to comment.