Skip to content

Commit f776811

Browse files
Peter010103Peter010103
committed
refactor: initial ROS2 refactoring
refactor: ROS2 temporal provider examples include files
1 parent 9d65d33 commit f776811

File tree

4 files changed

+182
-132
lines changed

4 files changed

+182
-132
lines changed

‎freyja_examples/CMakeLists.txt

Lines changed: 23 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,31 @@
1-
cmake_minimum_required(VERSION 2.8.3)
1+
cmake_minimum_required(VERSION 3.5)
22
project(freyja_examples)
3-
set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3" )
4-
5-
## Find catkin macros and libraries
6-
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
7-
## is used, also find other catkin packages
8-
find_package(catkin REQUIRED COMPONENTS
9-
geometry_msgs
10-
message_generation
11-
roscpp
12-
freyja_msgs
13-
std_msgs
14-
)
15-
3+
add_compile_options( -std=c++14 -O3 -ffast-math -Wall )
164

17-
catkin_package(
18-
# INCLUDE_DIRS include
19-
# LIBRARIES trajectory_provider
20-
CATKIN_DEPENDS geometry_msgs message_generation message_runtime roscpp std_msgs
21-
# DEPENDS system_lib
22-
)
5+
find_package(ament_cmake REQUIRED)
236

24-
###########
25-
## Build ##
26-
###########
7+
find_package(rclcpp REQUIRED)
8+
find_package(std_msgs REQUIRED)
9+
find_package(std_srvs REQUIRED)
10+
find_package(freyja_msgs REQUIRED)
2711

2812
include_directories(include)
29-
include_directories(
30-
${catkin_INCLUDE_DIRS}
13+
14+
add_executable(temporal_provider src/temporal_provider.cpp)
15+
16+
target_include_directories( temporal_provider PUBLIC
17+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
18+
$<INSTALL_INTERFACE:include>
19+
)
20+
21+
ament_target_dependencies( temporal_provider
22+
rclcpp
23+
freyja_msgs
24+
std_msgs
3125
)
3226

33-
# simple trajectory provider
34-
add_executable(temporal_provider_node src/temporal_provider.cpp)
35-
add_dependencies(temporal_provider_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
27+
install(TARGETS
28+
temporal_provider
29+
DESTINATION lib/${PROJECT_NAME})
3630

37-
target_link_libraries(temporal_provider_node
38-
${catkin_LIBRARIES}
39-
)
40-
31+
ament_package()

‎freyja_examples/include/temporal_provider.h

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
/* Provides a trajectory for the vehicle to follow.
2+
EXAMPLE FILE; ONLY FOR SUPPORT.
3+
Whatever you do here, output a time-based continuous function to follow.
4+
This node should generate a 7 vector: [pn pe pd vn ve vd yaw]' for the vehicle
5+
to follow. The controller currently listens to this reference trajectory
6+
and updates its knowledge of the "latest" reference point.
7+
8+
-- aj / 23rd Nov, 2017.
9+
*/
10+
11+
#include <chrono>
12+
#include <cmath>
13+
#include <memory>
14+
15+
#include "rclcpp/rclcpp.hpp"
16+
#include "std_msgs/msg/u_int8.hpp"
17+
#include "freyja_msgs/msg/reference_state.hpp"
18+
19+
typedef freyja_msgs::msg::ReferenceState TrajRef;
20+
21+
#define DEG2RAD(D) ((D)*3.1415326/180.0)
22+
23+
using std::placeholders::_1;
24+
using namespace std::chrono_literals;
25+
26+
rclcpp::Time init_time;
27+
std::string traj_type;
28+
int agg_level;
29+
30+
class Temporal_Traj : public rclcpp::Node
31+
{
32+
TrajRef ref_state;
33+
rclcpp::Time init_time;
34+
35+
std::string traj_type;
36+
int agg_level;
37+
38+
public:
39+
Temporal_Traj();
40+
41+
rclcpp::Subscription<std_msgs::msg::UInt8>::SharedPtr time_reset_sub_;
42+
void timer_reset_cb( const std_msgs::msg::UInt8::SharedPtr msg );
43+
44+
rclcpp::Publisher<TrajRef>::SharedPtr traj_pub_;
45+
46+
rclcpp::TimerBase::SharedPtr traj_update_timer_;
47+
void traj_update_cb();
48+
49+
TrajRef getHoverReference( rclcpp::Duration cur_time );
50+
TrajRef getCircleReference( rclcpp::Duration cur_time, const int agg_level);
51+
TrajRef getDefaultReference( rclcpp::Duration cur_time );
52+
TrajRef getLemiscateReference( rclcpp::Duration cur_time, const int agg_level);
53+
54+
};
55+

‎freyja_examples/package.xml

Lines changed: 11 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,20 @@
11
<?xml version="1.0"?>
2-
<package>
2+
<package format="3">
33
<name>freyja_examples</name>
44
<version>0.0.0</version>
5-
<description>The examples package showing use cases</description>
6-
7-
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8-
<!-- Example: -->
9-
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10-
<maintainer email="aj@todo.todo">aj</maintainer>
11-
5+
<description>ROS2 Trajectory Examples</description>
6+
<maintainer email="hw527@cam.ac.uk">Peter Woo</maintainer>
127
<license>GPLv3</license>
138

9+
<buildtool_depend>ament_cmake</buildtool_depend>
1410

15-
<buildtool_depend>catkin</buildtool_depend>
16-
<build_depend>geometry_msgs</build_depend>
17-
<build_depend>message_generation</build_depend>
18-
<build_depend>roscpp</build_depend>
19-
<build_depend>freyja_msgs</build_depend>
20-
<build_depend>std_msgs</build_depend>
21-
22-
<run_depend>geometry_msgs</run_depend>
23-
<run_depend>message_runtime</run_depend>
24-
<run_depend>roscpp</run_depend>
25-
<run_depend>std_msgs</run_depend>
26-
<run_depend>message_generation</run_depend>
27-
28-
29-
<!-- The export tag contains other, unspecified, tags -->
30-
<export>
31-
<!-- Other tools can request additional information be placed here -->
11+
<depend>rclcpp</depend>
12+
<depend>std_msgs</depend>
13+
<depend>std_srvs</depend>
14+
<depend>freyja_msgs</depend>
3215

16+
<export>
17+
<build_type>ament_cmake</build_type>
3318
</export>
19+
3420
</package>

‎freyja_examples/src/temporal_provider.cpp

Lines changed: 93 additions & 75 deletions
Original file line numberDiff line numberDiff line change
@@ -9,30 +9,69 @@
99
1010
-- aj / 23rd Nov, 2017.
1111
*/
12-
#include <cmath>
13-
#include <ros/ros.h>
14-
#include <std_msgs/UInt8.h>
15-
#include <freyja_msgs/ReferenceState.h>
12+
13+
#include "temporal_provider.h"
1614

1715
#define ROS_NODE_NAME "trajectory_provider"
18-
typedef freyja_msgs::ReferenceState TrajRef;
16+
#define UPDATE_RATE 50 // update rate of the trajectory (Hz)
17+
18+
Temporal_Traj::Temporal_Traj() : Node( ROS_NODE_NAME )
19+
{
20+
init_time = now();
21+
22+
declare_parameter<std::string>("traj_type", traj_type);
23+
declare_parameter<int>("agg_level", agg_level);
1924

20-
#define DEG2RAD(D) ((D)*3.1415326/180.0)
25+
get_parameter("traj_type", traj_type);
26+
get_parameter("agg_level", agg_level);
2127

22-
// global decl for "start time". This can be reset by a callback
23-
ros::Time init_time;
28+
time_reset_sub_ = create_subscription<std_msgs::msg::UInt8>(
29+
"/reset_trajectory_time", 1, std::bind(&Temporal_Traj::timer_reset_cb, this, _1));
30+
31+
traj_pub_ = create_publisher<TrajRef>("/reference_state", 1);
32+
33+
traj_update_timer_ = create_wall_timer(
34+
20ms, std::bind(&Temporal_Traj::traj_update_cb, this));
35+
}
2436

25-
void timeResetCallback( const std_msgs::UInt8::ConstPtr &msg )
37+
void Temporal_Traj::timer_reset_cb( std_msgs::msg::UInt8::SharedPtr msg )
2638
{
27-
if( msg -> data == 1 )
28-
{
29-
init_time = ros::Time::now();
30-
ROS_WARN( "%s: Time reset requested!", ROS_NODE_NAME );
31-
}
39+
if (msg->data == 1)
40+
{
41+
init_time = now();
42+
RCLCPP_WARN(get_logger(), "%s: Time reset requested!", ROS_NODE_NAME);
43+
}
3244
}
3345

34-
// HOVER AT A POINT
35-
TrajRef getHoverReference( const ros::Duration &cur_time )
46+
void Temporal_Traj::traj_update_cb()
47+
{
48+
get_parameter("traj_type", traj_type);
49+
50+
if (traj_type == "hover")
51+
{
52+
ref_state = Temporal_Traj::getHoverReference(now() - init_time);
53+
RCLCPP_INFO(get_logger(), "Publishing Hover Reference States");
54+
}
55+
else if (traj_type == "circle")
56+
{
57+
ref_state = Temporal_Traj::getCircleReference(now() - init_time, agg_level);
58+
RCLCPP_INFO(get_logger(), "Publishing Circle Reference States");
59+
}
60+
else if (traj_type == "lemiscate")
61+
{
62+
ref_state = Temporal_Traj::getLemiscateReference(now() - init_time, agg_level);
63+
RCLCPP_INFO(get_logger(), "Publishing Lemiscate Reference States");
64+
}
65+
else
66+
{
67+
RCLCPP_ERROR(get_logger(), "Invalid trajectory parameters given.");
68+
}
69+
70+
traj_pub_->publish(ref_state);
71+
}
72+
73+
// HOVER AT A POINT
74+
TrajRef Temporal_Traj::getHoverReference( rclcpp::Duration cur_time )
3675
{
3776
TrajRef ref_state;
3877
ref_state.pn = 0.0;
@@ -45,19 +84,25 @@ TrajRef getHoverReference( const ros::Duration &cur_time )
4584
ref_state.an = 0.0;
4685
ref_state.ae = 0.0;
4786
ref_state.ad = 0.0;
48-
ref_state.header.stamp = ros::Time::now();
87+
ref_state.header.stamp = now();
4988
return ref_state;
5089
}
5190

52-
// CIRCLE: pn = A*sin(wt), pe = A*cos(wt), vn = A*w*cos(wt) ..
53-
TrajRef getCircleReference( const ros::Duration &cur_time, const int agg_level)
91+
/** CIRCLE:
92+
* pn = A*sin(wt)
93+
* pe = A*cos(wt)
94+
* vn = A*w*cos(wt)
95+
* vn = -A*w*sin(wt)
96+
*/
97+
TrajRef Temporal_Traj::getCircleReference( const rclcpp::Duration cur_time, const int agg_level)
5498
{
5599
// A is amplitude (radius); w angular rate such that 2pi/w = (seconds for one rev)
56100
float A = 0.5;
57101
float w = 0.5;
58102

59103
// Set A and w based on agg_level
60-
switch(agg_level) {
104+
switch(agg_level)
105+
{
61106
case 1 :
62107
break;
63108
case 2 :
@@ -69,30 +114,31 @@ TrajRef getCircleReference( const ros::Duration &cur_time, const int agg_level)
69114
w = 3;
70115
break;
71116
default :
72-
ROS_WARN_STREAM("Circle aggression " << agg_level << " not supported, defaulting to agg_level 1");
73-
}
117+
RCLCPP_WARN(get_logger(), "Circle aggression %d not supported, defaulting to agg_level 1", agg_level);
118+
}
74119

75-
float t = cur_time.toSec();
120+
float t = cur_time.seconds();
76121

77-
// Create reference state
78-
TrajRef ref_state;
79-
ref_state.header.stamp = ros::Time::now();
122+
// Create reference state
123+
TrajRef ref_state;
124+
ref_state.header.stamp = now();
80125

81-
ref_state.pn = A*std::sin( w*t );
82-
ref_state.pe = A*std::cos( w*t );
83-
ref_state.pd = -1.0;
84-
85-
ref_state.vn = A*w*std::cos( w*t );
86-
ref_state.ve = -A*w*std::sin( w*t );
87-
ref_state.vd = 0.0;
126+
ref_state.pn = A*std::sin( w*t );
127+
ref_state.pe = A*std::cos( w*t );
128+
ref_state.pd = -4.0;
88129

89-
ref_state.yaw = 0.0;
130+
ref_state.vn = A*w*std::cos( w*t );
131+
ref_state.ve = -A*w*std::sin( w*t );
132+
ref_state.vd = 0.0;
133+
134+
ref_state.yaw = 0.0;
135+
136+
// set an, ae, ad to second derivatives if needed for FF..
137+
return ref_state;
90138

91-
// set an, ae, ad to second derivatives if needed for FF..
92-
return ref_state;
93139
}
94140

95-
TrajRef getDefaultReference( const ros::Duration &cur_time )
141+
TrajRef Temporal_Traj::getDefaultReference( rclcpp::Duration cur_time )
96142
{
97143
TrajRef ref_state;
98144
ref_state.pn = 0.5;
@@ -105,48 +151,20 @@ TrajRef getDefaultReference( const ros::Duration &cur_time )
105151
ref_state.an = 0.0;
106152
ref_state.ae = 0.0;
107153
ref_state.ad = 0.0;
108-
ref_state.header.stamp = ros::Time::now();
109-
return ref_state;
154+
ref_state.header.stamp = now();
155+
156+
return ref_state;
110157
}
111158

112-
int main( int argc, char** argv )
113159
{
114-
ros::init( argc, argv, ROS_NODE_NAME );
115-
ros::NodeHandle nh, priv_nh("~");
116160

117-
/* Publisher for trajectory */
118-
ros::Publisher traj_pub;
119-
traj_pub = nh.advertise <TrajRef> ( "/reference_state", 1, true );
120-
121-
/* Create subscriber for resetting time -- restart the trajectory */
122-
ros::Subscriber time_reset_sub;
123-
time_reset_sub = nh.subscribe( "/reset_trajectory_time", 1, timeResetCallback );
124-
125-
std::string traj_type;
126-
priv_nh.param( "example_traj_type", traj_type, std::string("hover") );
127-
128-
/* How fast should a trajectory update be made? */
129-
ros::Rate update_rate(50);
130-
init_time = ros::Time::now();
131-
132-
while( ros::ok() )
133-
{
134-
TrajRef ref_state;
135-
if( traj_type == "circle1" )
136-
ref_state = getCircleReference( ros::Time::now() - init_time, 1 );
137-
else if( traj_type == "circle2" )
138-
ref_state = getCircleReference( ros::Time::now() - init_time, 2 );
139-
else if( traj_type == "circle3" )
140-
ref_state = getCircleReference( ros::Time::now() - init_time, 3 );
141-
else if( traj_type == "hover" )
142-
ref_state = getHoverReference( ros::Time::now() - init_time );
143-
else
144-
ref_state = getDefaultReference( ros::Time::now() - init_time );
145-
traj_pub.publish( ref_state );
146-
147-
ros::spinOnce();
148-
update_rate.sleep();
149-
}
150161

162+
163+
int main(int argc, char * argv[])
164+
{
165+
rclcpp::init(argc, argv);
166+
rclcpp::spin(std::make_shared<Temporal_Traj>());
167+
rclcpp::shutdown();
151168
return 0;
152169
}
170+

0 commit comments

Comments
 (0)