Skip to content

Commit 67c9850

Browse files
author
Erwin Lejeune
authored
Merge pull request #3 from cmrobotics/feature/tf_test
Feature: TF Tests
2 parents e504baf + 035f731 commit 67c9850

File tree

6 files changed

+438
-0
lines changed

6 files changed

+438
-0
lines changed

‎.circleci/config.yml‎

Lines changed: 157 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,157 @@
1+
# Simple colcon build and deploy
2+
version: 2.1
3+
4+
orbs:
5+
aws-s3: circleci/aws-s3@3.0.0
6+
7+
workflows:
8+
build, test and create:
9+
jobs:
10+
- build-test:
11+
filters:
12+
tags:
13+
ignore: /[0-9]*\.[0-9]*\.[0-9]/
14+
context:
15+
- ROS Pipelines
16+
- OpenVPN
17+
- create:
18+
requires:
19+
- build-test
20+
context:
21+
- ROS Pipelines
22+
23+
build, test, create and deploy:
24+
jobs:
25+
- build-test:
26+
filters:
27+
branches:
28+
ignore: /.*/
29+
tags:
30+
only: /[0-9]*\.[0-9]*\.[0-9]/
31+
context:
32+
- ROS Pipelines
33+
- OpenVPN
34+
- create:
35+
filters:
36+
branches:
37+
ignore: /.*/
38+
tags:
39+
only: /[0-9]*\.[0-9]*\.[0-9]/
40+
requires:
41+
- build-test
42+
context:
43+
- ROS Pipelines
44+
- deploy:
45+
filters:
46+
branches:
47+
ignore: /.*/
48+
tags:
49+
only: /[0-9]*\.[0-9]*\.[0-9]/
50+
requires:
51+
- build-test
52+
- create
53+
context:
54+
- AWS ECR Access
55+
56+
executors:
57+
simple-machine:
58+
machine:
59+
image: ubuntu-2004:202111-02
60+
working_directory: ~/workspace/src/linak_actuator_modbus
61+
simple-docker:
62+
docker:
63+
- image: gallaisoma/ros2-galactic:latest
64+
working_directory: ~/workspace/src/cmr_tests_utils
65+
jobs:
66+
build-test:
67+
executor: simple-machine
68+
steps:
69+
- checkout:
70+
path: ~/workspace/src/$CIRCLE_PROJECT_REPONAME
71+
- run:
72+
name: Install OpenVPN3
73+
command: |
74+
sudo apt update && sudo apt install apt-transport-https
75+
sudo wget https://swupdate.openvpn.net/repos/openvpn-repo-pkg-key.pub
76+
sudo apt-key add openvpn-repo-pkg-key.pub
77+
sudo wget -O /etc/apt/sources.list.d/openvpn3.list https://swupdate.openvpn.net/community/openvpn3/repos/openvpn3-focal.list
78+
sudo apt update && sudo apt install openvpn3
79+
- run:
80+
name: VPN setup
81+
background: true
82+
command: |
83+
echo $VPN_CLIENT_CONFIG | base64 --decode > /tmp/config.ovpn
84+
phone_home=$(ss -Hnto state established '( sport = :ssh )' | head -n1 | awk '{ split($4, a, ":"); print a[1] }')
85+
echo $phone_home
86+
if [ -n "$phone_home" ]; then
87+
echo -e "\nroute $phone_home 255.255.255.255 net_gateway" >> /tmp/config.ovpn
88+
fi
89+
echo "\nroute 169.254.0.0 255.255.0.0 net_gateway" >> /tmp/config.ovpn
90+
sudo openvpn3 session-start --config /tmp/config.ovpn > /tmp/openvpn.log
91+
- run:
92+
name: Wait for the connection to be established and check
93+
command: |
94+
until sudo openvpn3 sessions-list|grep "Client connected"; do
95+
echo "Attempting to connect to VPN server..."
96+
sleep 1;
97+
done
98+
- run:
99+
name: Start Docker
100+
command: |
101+
docker pull gallaisoma/ros2-galactic:latest
102+
docker run -e ROSDISTRO_INDEX_URL=$ROSDISTRO_INDEX_URL -v ~/workspace/src/$CIRCLE_PROJECT_REPONAME:/root/workspace/src/$CIRCLE_PROJECT_REPONAME --name ci_fun -dit gallaisoma/ros2-galactic:latest
103+
- run:
104+
name: Run build and test inside docker
105+
command: docker exec -it ci_fun /bin/bash -c " source /opt/ros/galactic/setup.bash && cd ~/workspace && curl -o aptly_repo_signing.key http://172.31.46.198/aptly_repo_signing.key && apt-key add aptly_repo_signing.key && echo 'deb http://172.31.46.198/ focal main' >> /etc/apt/sources.list && apt update && rosdep init && rosdep update && rosdep install --from-paths src --ignore-src -r -y && colcon build --packages-up-to $CIRCLE_PROJECT_REPONAME && source install/setup.bash "
106+
- run:
107+
name: Disconnect from OpenVPN
108+
command: |
109+
SESSION_PATH=$(sudo openvpn3 sessions-list | grep Path | awk -F': ' '{print $2}')
110+
echo $SESSION_PATH
111+
sudo openvpn3 session-manage --session-path $SESSION_PATH --disconnect
112+
when: always
113+
114+
- persist_to_workspace:
115+
root: ../..
116+
paths:
117+
- .
118+
create:
119+
executor: simple-docker
120+
steps:
121+
- attach_workspace:
122+
at: ~/workspace
123+
- run:
124+
name: Deploy the deb artifact
125+
command: |
126+
cd ~/workspace
127+
mkdir ~/.ssh/
128+
ssh-keyscan github.com >> ~/.ssh/known_hosts
129+
git clone git@github.com:cmrobotics/pipeline_scripts.git
130+
cd ~/workspace/pipeline_scripts
131+
./binarization.sh
132+
133+
- store_artifacts:
134+
path: ~/workspace/src/ros-galactic-cmr_tests_utils.deb
135+
136+
- persist_to_workspace:
137+
root: ~/workspace
138+
paths:
139+
- src/ros-galactic-cmr_tests_utils.deb
140+
141+
deploy:
142+
executor: simple-docker
143+
steps:
144+
- attach_workspace:
145+
at: ~/workspace
146+
- run:
147+
name: Renaming with tag name
148+
command: mv ~/workspace/src/ros-galactic-$CIRCLE_PROJECT_REPONAME.deb ~/workspace/src/ros-galactic-$CIRCLE_PROJECT_REPONAME-$CIRCLE_TAG.deb
149+
- aws-s3/copy:
150+
arguments: |
151+
--acl private
152+
aws-access-key-id: AWS_ACCESS_KEY_ID
153+
aws-region: AWS_REGION
154+
aws-secret-access-key: AWS_SECRET_ACCESS_KEY
155+
from: ~/workspace/src/ros-galactic-$CIRCLE_PROJECT_REPONAME-$CIRCLE_TAG.deb
156+
to: 's3://aptly-debian-files/ros-galactic-$CIRCLE_PROJECT_REPONAME.deb'
157+

‎CMakeLists.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,10 @@ find_package(lifecycle_msgs REQUIRED)
2222
find_package(rclcpp REQUIRED)
2323
find_package(rclcpp_action REQUIRED)
2424
find_package(example_interfaces REQUIRED)
25+
find_package(geometry_msgs REQUIRED)
26+
find_package(tf2_ros REQUIRED)
27+
find_package(tf2 REQUIRED)
28+
find_package(tf2_kdl REQUIRED)
2529

2630

2731
include_directories(include
@@ -30,6 +34,10 @@ include_directories(include
3034
${rclcpp_INCLUDE_DIRS}
3135
${rclcpp_action_INCLUDE_DIRS}
3236
${example_interfaces_INCLUDE_DIRS}
37+
${geometry_msgs_INCLUDE_DIRS}
38+
${tf2_ros_INCLUDE_DIRS}
39+
${tf2_INCLUDE_DIRS}
40+
${tf2_kdl_INCLUDE_DIRS}
3341
)
3442

3543
set(LIBRARY_NAME "${PROJECT_NAME}_lib")
@@ -47,6 +55,10 @@ target_link_libraries(${LIBRARY_NAME} INTERFACE
4755
${rclcpp_LIBRARIES}
4856
${rclcpp_action_LIBRARIES}
4957
${example_interfaces_LIBRARIES}
58+
${geometry_msgs_LIBRARIES}
59+
${tf2_ros_LIBRARIES}
60+
${tf2_LIBRARIES}
61+
${tf2_kdl_LIBRARIES}
5062
)
5163

5264
install(TARGETS ${LIBRARY_NAME}
@@ -67,6 +79,10 @@ set(dependencies
6779
"rclcpp"
6880
"rclcpp_action"
6981
"example_interfaces"
82+
"geometry_msgs"
83+
"tf2_ros"
84+
"tf2"
85+
"tf2_kdl"
7086
)
7187

7288
if(BUILD_TESTING)

‎include/cmr_tests_utils/basic_tf_broadcaster_node_test.hpp‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
#ifndef BASIC_TF_BROADCASTER_NODE_TEST_HPP
2+
#define BASIC_TF_BROADCASTER_NODE_TEST_HPP
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
#include "geometry_msgs/msg/transform_stamped.hpp"
6+
#include "geometry_msgs/msg/pose_stamped.hpp"
7+
#include "tf2_ros/transform_broadcaster.h"
8+
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
9+
#include "tf2_ros/buffer.h"
10+
#include "tf2_ros/transform_listener.h"
11+
#include "tf2/utils.h"
12+
13+
namespace cmr_tests_utils
14+
{
15+
16+
class BasicTfBroadcasterNodeTest: public rclcpp::Node {
17+
18+
public:
19+
20+
BasicTfBroadcasterNodeTest(std::string node_name, geometry_msgs::msg::TransformStamped initial_transform,
21+
unsigned int publish_period_ms): rclcpp::Node(node_name)
22+
{
23+
clock_ = get_clock();
24+
buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
25+
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
26+
broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
27+
transform_ = initial_transform;
28+
timer_ = this->create_wall_timer(
29+
std::chrono::milliseconds(publish_period_ms),
30+
std::bind(&BasicTfBroadcasterNodeTest::broadcast_transform_, this)
31+
);
32+
}
33+
34+
const geometry_msgs::msg::TransformStamped& get_transform () const
35+
{
36+
std::lock_guard<std::mutex> lock(tf_mutex_);
37+
return transform_;
38+
}
39+
40+
void set_transform(const geometry_msgs::msg::TransformStamped & transform)
41+
{
42+
std::lock_guard<std::mutex> lock(tf_mutex_);
43+
transform_ = transform;
44+
}
45+
46+
private:
47+
48+
void broadcast_transform_()
49+
{
50+
RCLCPP_ERROR(get_logger(), "Fuck you");
51+
std::lock_guard<std::mutex> lock(tf_mutex_);
52+
transform_.header.stamp = clock_->now();
53+
broadcaster_->sendTransform(transform_);
54+
}
55+
56+
rclcpp::TimerBase::SharedPtr timer_;
57+
rclcpp::Duration transform_tolerance_ {0, 0};
58+
std::shared_ptr<tf2_ros::Buffer> buffer_;
59+
std::shared_ptr<tf2_ros::TransformListener> listener_;
60+
std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;
61+
geometry_msgs::msg::TransformStamped transform_;
62+
rclcpp::Clock::SharedPtr clock_;
63+
mutable std::mutex tf_mutex_;
64+
};
65+
66+
}
67+
68+
#endif

‎include/cmr_tests_utils/basic_tf_listener_node_test.hpp‎

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
#ifndef BASIC_TF_LISTENER_NODE_TEST_HPP
2+
#define BASIC_TF_LISTENER_NODE_TEST_HPP
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
#include "tf2/LinearMath/Quaternion.h"
6+
#include "tf2/LinearMath/Transform.h"
7+
#include "geometry_msgs/msg/transform_stamped.hpp"
8+
#include "geometry_msgs/msg/pose_stamped.hpp"
9+
#include "tf2_ros/transform_broadcaster.h"
10+
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
11+
#include "tf2_ros/buffer.h"
12+
#include "tf2_ros/transform_listener.h"
13+
#include "tf2/utils.h"
14+
15+
namespace cmr_tests_utils
16+
{
17+
18+
class BasicTfListenerNodeTest: public rclcpp::Node {
19+
20+
public:
21+
22+
BasicTfListenerNodeTest(std::string node_name, double transform_tolerance): rclcpp::Node(node_name)
23+
{
24+
clock_ = get_clock();
25+
buffer_ = std::make_shared<tf2_ros::Buffer>(clock_);
26+
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);
27+
transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance);
28+
}
29+
30+
bool transform_pose(const std::string target_frame,
31+
const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) const
32+
{
33+
if (in_pose.header.frame_id == target_frame) {
34+
out_pose = in_pose;
35+
return true;
36+
}
37+
38+
try {
39+
if (!buffer_->canTransform(target_frame, in_pose.header.frame_id, rclcpp::Time(), transform_tolerance_)) {
40+
RCLCPP_ERROR(get_logger(), "Transform between %s and %s was not available", in_pose.header.frame_id.c_str(), target_frame.c_str());
41+
return false;
42+
}
43+
// Transform is a blocking call that will hang forever if frames don't exist, so we call canTransform first
44+
buffer_->transform(in_pose, out_pose, target_frame);
45+
return true;
46+
} catch (tf2::ExtrapolationException & ex) {
47+
geometry_msgs::msg::TransformStamped transform;
48+
49+
if (this->lookup_transform(transform, target_frame, in_pose.header.frame_id))
50+
{
51+
tf2::doTransform(in_pose, out_pose, transform);
52+
return true;
53+
}
54+
}
55+
56+
return false;
57+
}
58+
59+
bool lookup_transform(geometry_msgs::msg::TransformStamped & transform, std::string target_frame, std::string source_frame) const
60+
{
61+
try {
62+
if (!buffer_->canTransform(target_frame, source_frame, rclcpp::Time(), transform_tolerance_)) {
63+
RCLCPP_ERROR(get_logger(), "Transform between %s and %s was not available", source_frame.c_str(), target_frame.c_str());
64+
return false;
65+
}
66+
67+
// Lookup Transform is a blocking call that will hang forever if frames don't exist, so we call canTransform first
68+
transform = buffer_->lookupTransform(target_frame, source_frame, rclcpp::Time(), transform_tolerance_);
69+
70+
} catch (tf2::TransformException & ex) {
71+
RCLCPP_ERROR(
72+
rclcpp::get_logger("tf_help"),
73+
"Exception in transform_pose: %s",
74+
ex.what()
75+
);
76+
return false;
77+
}
78+
79+
return true;
80+
}
81+
82+
private:
83+
84+
// ROS
85+
rclcpp::Duration transform_tolerance_ {0, 0};
86+
std::shared_ptr<tf2_ros::Buffer> buffer_;
87+
std::shared_ptr<tf2_ros::TransformListener> listener_;
88+
rclcpp::Clock::SharedPtr clock_;
89+
};
90+
91+
}
92+
93+
#endif

‎test/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,4 +20,10 @@ set(TEST_NAME service_communication_test)
2020
ament_add_gtest(${TEST_NAME} ${TEST_NAME}.cpp)
2121
target_link_libraries(${TEST_NAME} ${LIBRARY_NAME})
2222
ament_target_dependencies(${TEST_NAME} ${dependencies})
23+
target_include_directories(${TEST_NAME} PRIVATE "../include" "include")
24+
25+
set(TEST_NAME transforms_test)
26+
ament_add_gtest(${TEST_NAME} ${TEST_NAME}.cpp)
27+
target_link_libraries(${TEST_NAME} ${LIBRARY_NAME})
28+
ament_target_dependencies(${TEST_NAME} ${dependencies})
2329
target_include_directories(${TEST_NAME} PRIVATE "../include" "include")

0 commit comments

Comments
 (0)