├── .gitignore
├── src
    ├── wiln_node.cpp
    └── wiln.cpp
├── config
    └── wiln.yaml
├── wiln.repos
├── launch
    └── wiln.launch.py
├── CMakeLists.txt
├── package.xml
├── include
    ├── utils.hpp
    └── wiln.hpp
├── scripts
    └── svg_to_trajectory.py
├── README.md
└── trajectories
    └── square.ltr


/.gitignore:
--------------------------------------------------------------------------------
1 | .idea/
2 | cmake-build-debug/
3 | data/
4 | .ipynb_checkpoints/
5 | 


--------------------------------------------------------------------------------
/src/wiln_node.cpp:
--------------------------------------------------------------------------------
 1 | #include <rclcpp/rclcpp.hpp>
 2 | #include "wiln.hpp"
 3 | 
 4 | int main(int argc, char **argv)
 5 | {
 6 |     rclcpp::init(argc, argv);
 7 |     rclcpp::executors::MultiThreadedExecutor executor;
 8 |     auto wiln_node = std::make_shared<WilnNode>();
 9 |     executor.add_node(wiln_node);
10 |     executor.spin();
11 |     rclcpp::shutdown();
12 | 
13 |     return 0;
14 | }
15 | 


--------------------------------------------------------------------------------
/config/wiln.yaml:
--------------------------------------------------------------------------------
 1 | /**:
 2 |   wiln_node:
 3 |     ros__parameters:
 4 |       odom_topic: "/mapping/icp_odom"
 5 |       follow_path_topic: "/follow_path"
 6 |       distance_between_waypoints: 0.1
 7 |       angle_between_waypoints: 0.1
 8 |       trajectory_speed: 1.5
 9 |       smoothing_window_size: 9
10 |       loop_closure_linear_tolerance: 5.0
11 |       loop_closure_angular_tolerance: 1.58
12 | 
13 | 


--------------------------------------------------------------------------------
/wiln.repos:
--------------------------------------------------------------------------------
 1 | repositories:
 2 |   wiln:
 3 |     type: git
 4 |     url: git@github.com:norlab-ulaval/wiln.git
 5 |     version: humble
 6 |   libpointmatcher_ros:
 7 |     type: git
 8 |     url: git@github.com:norlab-ulaval/libpointmatcher_ros.git
 9 |     version: humble
10 |   norlab_controllers_ros:
11 |     type: git
12 |     url: git@github.com:norlab-ulaval/norlab_controllers_ros.git
13 |     version: humble
14 |   norlab_icp_mapper_ros:
15 |     type: git
16 |     url: git@github.com:norlab-ulaval/norlab_icp_mapper_ros.git
17 |     version: humble
18 |   pointcloud_motion_deskew:
19 |     type: git
20 |     url: git@github.com:norlab-ulaval/pointcloud_motion_deskew.git
21 |     version: humble
22 | 


--------------------------------------------------------------------------------
/launch/wiln.launch.py:
--------------------------------------------------------------------------------
 1 | import os
 2 | 
 3 | from ament_index_python.packages import get_package_share_directory
 4 | from launch import LaunchDescription
 5 | from launch.substitutions import LaunchConfiguration
 6 | from launch.actions import DeclareLaunchArgument
 7 | from launch_ros.actions import Node
 8 | 
 9 | def generate_launch_description():
10 | 
11 |     namespace = LaunchConfiguration('wiln_ns')
12 |     namespace_launch_arg = DeclareLaunchArgument(
13 |         'wiln_ns',
14 |         default_value='wiln'
15 |     )
16 | 
17 |     share_folder = get_package_share_directory('wiln')
18 |     config_file = os.path.join(share_folder, "config", "_wiln.yaml")
19 | 
20 |     wiln_node = Node(
21 |         package='wiln',
22 |         executable='wiln_node',
23 |         name='wiln_node',
24 |         namespace=namespace,
25 |         output='screen',
26 |         parameters=[config_file]
27 |     )
28 | 
29 |     return LaunchDescription([
30 |         namespace_launch_arg,
31 |         wiln_node
32 |     ])


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(wiln)
 3 | 
 4 | find_package(ament_cmake REQUIRED)
 5 | find_package(rclcpp REQUIRED)
 6 | find_package(geometry_msgs REQUIRED)
 7 | find_package(std_srvs REQUIRED)
 8 | find_package(std_msgs REQUIRED)
 9 | find_package(tf2_geometry_msgs REQUIRED)
10 | find_package(tf2 REQUIRED)
11 | find_package(rclcpp_action REQUIRED)
12 | find_package(nav_msgs REQUIRED)
13 | find_package(norlab_icp_mapper_ros REQUIRED)
14 | find_package(wiln_msgs REQUIRED)
15 | find_package(controller_msgs REQUIRED)
16 | find_package(tf2_geometry_msgs REQUIRED)
17 | 
18 | include_directories(include)
19 | 
20 | add_executable(wiln_node src/wiln_node.cpp src/wiln.cpp)
21 | ament_target_dependencies(wiln_node
22 |     rclcpp
23 |     std_msgs
24 |     geometry_msgs
25 |     nav_msgs
26 |     std_srvs
27 |     tf2_ros
28 |     tf2
29 |     rclcpp_action
30 |     norlab_icp_mapper_ros
31 |     wiln_msgs
32 |     controller_msgs
33 |     tf2_geometry_msgs
34 | )
35 | 
36 | install(TARGETS wiln_node
37 |         DESTINATION lib/${PROJECT_NAME}
38 | )
39 | 
40 | install(DIRECTORY launch
41 |         DESTINATION share/${PROJECT_NAME})
42 | 
43 | ament_package()
44 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>wiln</name>
 5 |   <version>0.1.0</version>
 6 |   <description>A ROS node that implements Norlab's teach-and-repeat framework.</description>
 7 | 
 8 |   <!-- One maintainer tag required, multiple allowed, one person per tag -->
 9 |   <!-- Example:  -->
10 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
11 |   <maintainer email="jean-michel.fortin@norlab.ulaval.ca">Jean-Michel Fortin</maintainer>
12 |   <maintainer email="nicolas.samson@norlab.ulaval.ca">Nicolas Samson</maintainer>
13 | 
14 |   <maintainer email="simon-pierre.deschenes.1@ulaval.ca">Simon-Pierre Deschênes</maintainer>
15 |   <maintainer email="dominic.baril.1@ulaval.ca">Dominic Baril</maintainer>
16 | 
17 | 
18 |   <!-- One license tag required, multiple allowed, one license per tag -->
19 |   <!-- Commonly used license strings: -->
20 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
21 |   <license>BSD</license>
22 | 
23 | 
24 |   <!-- Url tags are optional, but multiple are allowed, one per tag -->
25 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
26 |   <!-- Example: -->
27 |   <!-- <url type="website">http://wiki.ros.org/WILN</url> -->
28 | 
29 | 
30 |   <!-- Author tags are optional, multiple are allowed, one per tag -->
31 |   <!-- Authors do not have to be maintainers, but could be -->
32 |   <!-- Example: -->
33 |   <author email="simon-pierre.deschenes.1@ulaval.ca">Simon-Pierre Deschênes</author>
34 |   <author email="dominic.baril.1@ulaval.ca">Dominic Baril</author>
35 | 
36 |   <buildtool_depend>ament_cmake</buildtool_depend>
37 | 
38 |   <depend>rclcpp</depend>
39 |   <depend>std_msgs</depend>
40 |   <depend>sensor_msgs</depend>
41 |   <depend>geometry_msgs</depend>
42 |   <depend>std_srvs</depend>
43 |   <depend>tf2_ros</depend>
44 |   <depend>tf2</depend>
45 |   <depend>norlab_icp_mapper_ros</depend>
46 |   <depend>wiln_msgs</depend>
47 |   <depend>controller_msgs</depend>
48 |   <depend>tf2_geometry_msgs</depend>
49 | 
50 |   <!-- The export tag contains other, unspecified, tags -->
51 |   <export>
52 |     <!-- Other tools can request additional information be placed here -->
53 |     <build_type>ament_cmake</build_type>
54 | 
55 |   </export>
56 | </package>
57 | 


--------------------------------------------------------------------------------
/include/utils.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef UTILS_HPP
  2 | #define UTILS_HPP
  3 | 
  4 | #include <cmath>
  5 | #include <geometry_msgs/msg/pose_stamped.hpp>
  6 | #include <tf2/LinearMath/Quaternion.h>
  7 | #include <tf2/LinearMath/Matrix3x3.h>
  8 | #include <nav_msgs/msg/path.hpp>
  9 | 
 10 | inline double wrapToPi(double angle) {
 11 |     return fmod(angle + M_PI, 2.0 * M_PI) - M_PI;
 12 | }
 13 | 
 14 | std::tuple<double, double, double> quatToRPY(const geometry_msgs::msg::Quaternion &q)
 15 | {
 16 |     tf2::Quaternion quat(q.x, q.y, q.z, q.w);
 17 |     double roll, pitch, yaw;
 18 |     tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
 19 |     return std::make_tuple(roll, pitch, yaw);
 20 | }
 21 | 
 22 | std::pair<float, float> diffBetweenPoses(const geometry_msgs::msg::Pose &pose1, const geometry_msgs::msg::Pose &pose2)
 23 | {
 24 |     float dx = pose2.position.x - pose1.position.x;
 25 |     float dy = pose2.position.y - pose1.position.y;
 26 |     auto [roll1, pitch1, yaw1] = quatToRPY(pose1.orientation);
 27 |     auto [roll2, pitch2, yaw2] = quatToRPY(pose2.orientation);
 28 |     float diff_ang = wrapToPi(yaw2 - yaw1);
 29 |     float diff_lin = std::sqrt(dx * dx + dy * dy);
 30 |     return std::make_pair(diff_lin, diff_ang);
 31 | }
 32 | 
 33 | nav_msgs::msg::Path reversePath(const nav_msgs::msg::Path &path)
 34 | {
 35 |     nav_msgs::msg::Path reversedPath = path;
 36 |     std::reverse(reversedPath.poses.begin(), reversedPath.poses.end());
 37 |     return reversedPath;
 38 | }
 39 | 
 40 | geometry_msgs::msg::Quaternion flipQuaternion(const geometry_msgs::msg::Quaternion &q)
 41 | {
 42 |     tf2::Quaternion quat(q.x, q.y, q.z, q.w);
 43 |     tf2::Matrix3x3 m(quat);
 44 |     double roll, pitch, yaw;
 45 |     m.getRPY(roll, pitch, yaw);
 46 |     tf2::Quaternion flippedQuat;
 47 |     flippedQuat.setRPY(-roll, -pitch, yaw + M_PI);
 48 |     return tf2::toMsg(flippedQuat);
 49 | }
 50 | 
 51 | nav_msgs::msg::Path flipPath(const nav_msgs::msg::Path &path)
 52 | {
 53 |     nav_msgs::msg::Path flippedPath = path;
 54 |     for (auto &pose : flippedPath.poses)
 55 |     {
 56 |         pose.pose.orientation = flipQuaternion(pose.pose.orientation);
 57 |     }
 58 |     return flippedPath;
 59 | }
 60 | 
 61 | nav_msgs::msg::Path smoothPathLowPass(const nav_msgs::msg::Path &roughPath, int windowSize)
 62 | {
 63 |     nav_msgs::msg::Path smoothPath(roughPath);
 64 | 
 65 |     // Check that window size is odd
 66 |     if (windowSize % 2 == 0)
 67 |     {
 68 |         windowSize++;
 69 |     }
 70 | 
 71 |     // Iterate through each pose in the roughPath
 72 |     for (size_t j = 0; j < roughPath.poses.size(); ++j)
 73 |     {
 74 |         double sumX = 0, sumY = 0, sumZ = 0;
 75 |         int distFromBounds = std::min(j, (int)roughPath.poses.size() - j - 1);
 76 |         int dynWindow = std::min(windowSize, distFromBounds*2+1);
 77 | 
 78 |         // Sum positions within the dynamic window
 79 |         for (size_t k = j - dynWindow / 2; k <= j + dynWindow / 2; ++k)
 80 |         {
 81 |             sumX += roughPath.poses[k].pose.position.x;
 82 |             sumY += roughPath.poses[k].pose.position.y;
 83 |             sumZ += roughPath.poses[k].pose.position.z;
 84 |         }
 85 | 
 86 |         // Compute the average and assign it to the smoothed path
 87 |         smoothPath.poses[j].pose.position.x = sumX / dynWindow;
 88 |         smoothPath.poses[j].pose.position.y = sumY / dynWindow;
 89 |         smoothPath.poses[j].pose.position.z = sumZ / dynWindow;
 90 |     }
 91 | 
 92 |     return smoothPath;
 93 | }
 94 | 
 95 | nav_msgs::msg::Path removePathOverlap(const nav_msgs::msg::Path &loopPath)
 96 | {
 97 |     nav_msgs::msg::Path cutLoopPath(loopPath);
 98 |     
 99 |     auto first_itr = cutLoopPath.poses.begin();
100 |     auto last_pose = std::prev(cutLoopPath.poses.end())->pose;
101 | 
102 |     while (cutLoopPath.poses.size() > 2)
103 |     {
104 |         auto [lin_dist1, ang_dist1] = diffBetweenPoses(last_pose, first_itr->pose);
105 |         auto [lin_dist2, ang_dist2] = diffBetweenPoses(last_pose, std::next(first_itr)->pose);
106 | 
107 |         if (lin_dist1 > lin_dist2)
108 |         {
109 |             first_itr = cutLoopPath.poses.erase(first_itr);
110 |         }
111 |         else
112 |         {
113 |             break;
114 |         }
115 |     }
116 |     return cutLoopPath;
117 | }
118 | 
119 | #endif // UTILS_HPP
120 | 


--------------------------------------------------------------------------------
/scripts/svg_to_trajectory.py:
--------------------------------------------------------------------------------
  1 | import os
  2 | import numpy as np
  3 | import matplotlib.pyplot as plt
  4 | from svgpathtools import svg2paths
  5 | import argparse
  6 | 
  7 | 
  8 | def load_path_from_svg(file_path, num_points=1000):
  9 |     
 10 |     first_path, _ = svg2paths(file_path)[0]  # Get the first path object from the SVG file
 11 |     complex_points = [first_path.point(t) for t in np.linspace(0, 1, num_points)]
 12 |     xy_points = np.array([[pt.real, pt.imag] for pt in complex_points])
 13 |     return xy_points
 14 | 
 15 | 
 16 | def scale_path(points, target_width):
 17 | 
 18 |     original_width = np.ptp(points[:, 0])  # Width is the range of x-coordinates
 19 |     scaling_factor = target_width / original_width if original_width > 0 else 1.0
 20 |     scaled_points = (points - np.min(points, axis=0)) * scaling_factor
 21 |     return scaled_points
 22 | 
 23 | 
 24 | def resample_path(points, avg_distance):
 25 | 
 26 |     if len(points) < 2:
 27 |         return points  # No resampling needed for single point
 28 | 
 29 |     resampled_points = [points[0]]  # Start with the first point
 30 |     last_point = points[0]
 31 | 
 32 |     for point in points[1:]:
 33 |         distance = np.linalg.norm(point - last_point)
 34 |         if distance >= avg_distance:
 35 |             resampled_points.append(point)
 36 |             last_point = point
 37 | 
 38 |     return np.array(resampled_points)
 39 | 
 40 | 
 41 | def compute_orientations(points, threshold=1e-6):
 42 |     
 43 |     angles = []
 44 |     for i in range(1, len(points)):
 45 |         dx = points[i][0] - points[i-1][0]
 46 |         dy = points[i][1] - points[i-1][1]
 47 |         if np.linalg.norm([dx, dy]) < threshold:
 48 |             angle = angles[-1]  # Keep the same angle if movement is small
 49 |         else:
 50 |             angle = np.arctan2(dy, dx)
 51 |         angles.append(angle)
 52 | 
 53 |     angles.insert(0, angles[0])  # First point's angle is the same as the first segment
 54 |     xy_yaw_path = np.array([(x, y, angle) for (x, y), angle in zip(points, angles)])
 55 |     return xy_yaw_path
 56 | 
 57 | 
 58 | def visualize_path(xy_yaw_path, target_width, title="SVG Path Visualization"):
 59 |     
 60 |     plt.figure(figsize=(10, 10)) # Make plot a bit larger
 61 |     plt.plot([p[0] for p in xy_yaw_path], [p[1] for p in xy_yaw_path],
 62 |                 marker='.', markersize=2, linestyle='-', color='blue', label='Generated Path')
 63 |     plt.gca().set_aspect('equal', adjustable='box')
 64 |     plt.title(title)
 65 |     plt.xlabel("X (meters)")
 66 |     plt.ylabel("Y (meters)")
 67 |     plt.grid(True)
 68 | 
 69 |     arrow_length = target_width * 0.04 # Make arrow length relative to scale
 70 |     head_width = arrow_length * 0.25
 71 |     head_length = arrow_length * 0.35
 72 | 
 73 |     for i in range(0, len(xy_yaw_path)):
 74 |         x, y, angle = xy_yaw_path[i]
 75 |         plt.arrow(x, y,
 76 |                     arrow_length * np.cos(angle), arrow_length * np.sin(angle),
 77 |                     head_width=head_width, head_length=head_length, fc='red', ec='red', alpha=0.7)
 78 |     plt.legend()
 79 |     plt.show()
 80 | 
 81 | 
 82 | def convert_to_ltr(xy_yaw_path, output_directory, input_svg):
 83 |     
 84 |     output_file = os.path.join(output_directory, os.path.splitext(os.path.basename(input_svg))[0] + ".ltr")
 85 |     with open(output_file, 'w') as ltr_file:
 86 |         ltr_file.write("#############################\n")
 87 |         ltr_file.write(f"frame_id : map\n")  # Assuming 'map' as the frame_id
 88 |         for x, y, angle in xy_yaw_path:
 89 |             ltr_file.write(f"{x},{y},0.0,0.0,0.0,{np.sin(angle / 2)},{np.cos(angle / 2)}\n")
 90 |           
 91 |           
 92 | def parse_arguments():
 93 |     
 94 |     parser = argparse.ArgumentParser(description="Convert SVG path to trajectory with optional visualization.")
 95 |     parser.add_argument("input_svg", type=str, help="Path to the input SVG file.")
 96 |     parser.add_argument("--output", "-o", type=str, default="../trajectories", help="Directory to save the output LTR file.")
 97 |     parser.add_argument("--width", "-w", type=float, default=10.0, help="Target width in meters for scaling the path.")
 98 |     parser.add_argument("--spacing", "-s", type=float, default=0.1, help="Desired distance between waypoints in meters.")
 99 |     parser.add_argument("--preview", "-p", action="store_true", help="Show a plot preview of the path.")
100 | 
101 |     return parser.parse_args()
102 |             
103 | 
104 | def main():
105 | 
106 |     args = parse_arguments()
107 |     
108 |     svg_path = load_path_from_svg(args.input_svg)
109 |     scaled_path = scale_path(svg_path, args.width)
110 |     sampled_path = resample_path(scaled_path, args.spacing)
111 |     xy_yaw_path = compute_orientations(sampled_path)
112 | 
113 |     if args.preview:
114 |         visualize_path(xy_yaw_path, args.width, title=f"SVG Path from '{os.path.basename(args.input_svg)}'")
115 | 
116 |     convert_to_ltr(xy_yaw_path, args.output, args.input_svg)
117 | 
118 | 
119 | if __name__ == "__main__":
120 |     main()


--------------------------------------------------------------------------------
/include/wiln.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef WILN_NODE_HPP
  2 | #define WILN_NODE_HPP
  3 | 
  4 | #include <std_srvs/srv/empty.hpp>
  5 | #include <tf2/LinearMath/Quaternion.h>
  6 | #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
  7 | 
  8 | #include <wiln_msgs/srv/save_map_traj.hpp>
  9 | #include <wiln_msgs/srv/load_map_traj.hpp>
 10 | #include <wiln_msgs/srv/play_loop.hpp>
 11 | #include <norlab_icp_mapper_ros/srv/save_map.hpp>
 12 | #include <norlab_icp_mapper_ros/srv/load_map.hpp>
 13 | 
 14 | #include <geometry_msgs/msg/twist.hpp>
 15 | #include <nav_msgs/msg/odometry.hpp>
 16 | #include <nav_msgs/msg/path.hpp>
 17 | #include <std_msgs/msg/u_int8.hpp>
 18 | 
 19 | #include <rclcpp_action/rclcpp_action.hpp>
 20 | #include <controller_msgs/action/follow_path.hpp>
 21 | 
 22 | enum State
 23 | {
 24 |     IDLE,
 25 |     RECORDING,
 26 |     PLAYING
 27 | };
 28 | 
 29 | class WilnNode : public rclcpp::Node
 30 | {
 31 | public:
 32 |     WilnNode();
 33 | 
 34 | private:
 35 |     State currentState;
 36 |     nav_msgs::msg::Path plannedTrajectory;
 37 |     nav_msgs::msg::Path realTrajectory;
 38 |     geometry_msgs::msg::PoseStamped robotPose;
 39 |     std::mutex robotPoseLock;
 40 | 
 41 |     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSubscription;
 42 |     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr plannedTrajectoryPublisher;
 43 |     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr realTrajectoryPublisher;
 44 |     rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr statePublisher;
 45 | 
 46 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr startRecordingService;
 47 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stopRecordingService;
 48 |     rclcpp::Service<wiln_msgs::srv::SaveMapTraj>::SharedPtr saveMapTrajService;
 49 |     rclcpp::Service<wiln_msgs::srv::LoadMapTraj>::SharedPtr loadMapTrajService;
 50 |     rclcpp::Service<wiln_msgs::srv::LoadMapTraj>::SharedPtr loadMapTrajFromEndService;
 51 |     rclcpp::Service<wiln_msgs::srv::PlayLoop>::SharedPtr playLoopService;
 52 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr playLineService;
 53 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr cancelTrajectoryService;
 54 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr smoothTrajectoryService;
 55 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr clearTrajectoryService;
 56 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reverseTrajectoryService;
 57 |     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr flipTrajectoryService;
 58 | 
 59 |     rclcpp::Client<std_srvs::srv::Empty>::SharedPtr enableMappingClient;
 60 |     rclcpp::Client<std_srvs::srv::Empty>::SharedPtr disableMappingClient;
 61 |     rclcpp::Client<norlab_icp_mapper_ros::srv::SaveMap>::SharedPtr saveMapClient;
 62 |     rclcpp::Client<norlab_icp_mapper_ros::srv::LoadMap>::SharedPtr loadMapClient;
 63 |     rclcpp_action::Client<controller_msgs::action::FollowPath>::SharedPtr followPathClient;
 64 | 
 65 |     rclcpp::TimerBase::SharedPtr stateTimer;
 66 |     rclcpp::TimerBase::SharedPtr updateTimer;
 67 | 
 68 |     const int FRAME_ID_START_POSITION = 11;
 69 |     const std::string TRAJECTORY_DELIMITER = "#############################";
 70 |     std::string odomTopic;
 71 |     std::string followPathTopic;
 72 |     float distanceBetweenWaypoints;
 73 |     float angleBetweenWaypoints;
 74 |     float trajectorySpeed;
 75 |     int smoothingWindowSize;
 76 |     float loopClosureLinearTolerance;
 77 |     float loopClosureAngularTolerance;
 78 | 
 79 |     void initParameters();
 80 |     void updateParameters();
 81 |     void initSubscribers();
 82 |     void initPublishers();
 83 |     void initServices();
 84 |     void initClients();
 85 |     void initTimers();
 86 |     void odomCallback(const nav_msgs::msg::Odometry &odomIn);
 87 |     void updatePlannedTrajectory(const geometry_msgs::msg::PoseStamped &currentPose);
 88 |     void updateRealTrajectory(const geometry_msgs::msg::PoseStamped &currentPose);
 89 |     void startRecordingServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 90 |     void stopRecordingServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 91 |     void clearTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 92 |     void reverseTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 93 |     void flipTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 94 |     void smoothTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 95 |     void cancelTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
 96 |     void saveLTRServiceCallback(const std::shared_ptr<wiln_msgs::srv::SaveMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::SaveMapTraj::Response> res);
 97 |     bool saveLTR(std::string fileName);
 98 |     void loadLTRServiceCallback(const std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Response> res);
 99 |     void loadLTRFromEndServiceCallback(const std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Response> res);
100 |     bool loadLTR(std::string fileName, bool fromEnd);
101 |     void enableMapping();
102 |     void disableMapping();
103 |     void saveMap(std::string folderName);
104 |     void loadMap(geometry_msgs::msg::Pose pose, std::string fileNameMap);
105 |     void publishPlannedTrajectory();
106 |     void publishRealTrajectory();
107 |     void publishState();
108 |     void playLineServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res);
109 |     void playLine();
110 |     void playLoopServiceCallback(const std::shared_ptr<wiln_msgs::srv::PlayLoop::Request> req, std::shared_ptr<wiln_msgs::srv::PlayLoop::Response> res);
111 |     void playLoop(int nbLoops);
112 |     void sendFollowPathAction(nav_msgs::msg::Path &path);
113 |     void goalResponseCallback(const rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::SharedPtr &trajectoryGoalHandle);
114 |     void trajectoryFeedbackCallback(rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::SharedPtr, const std::shared_ptr<const controller_msgs::action::FollowPath::Feedback> feedback);
115 |     void trajectoryResultCallback(const rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::WrappedResult &trajectory_result);
116 | };
117 | 
118 | #endif // WILN_NODE_HPP
119 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # WILN - Weather-Invariant Lidar-based Navigation
  2 | 
  3 | [![DOI](https://zenodo.org/badge/DOI/10.55417/fr.2022050.svg)](https://doi.org/10.55417/fr.2022050)
  4 | [![ROS1](https://img.shields.io/badge/ROS1-melodic-blue?labelColor=blue&logo=ROS)](http://wiki.ros.org/melodic)
  5 | [![ROS2](https://img.shields.io/badge/ROS2-humble-blue?labelColor=blue&logo=ROS)](https://docs.ros.org/en/humble)
  6 | 
  7 | WILN is a teach-and-repeat framework relying on lidar-based navigation to perform autonomous route repeating. This is the framework that was used in the ["Kilometer-scale autonomous navigation in subarctic forests: challenges and lessons learned"](https://norlab.ulaval.ca/publications/field-report-ltr/) field report. An overview of the system demonstration is presented in the following video :
  8 | 
  9 | [![SNOW WILN deployment](https://img.youtube.com/vi/W8TdAoeNv4U/0.jpg)](https://www.youtube.com/watch?v=W8TdAoeNv4U)
 10 | 
 11 | The WILN system interfaces two libraries/packages: [`libpointmatcher`](https://github.com/norlab-ulaval/libpointmatcher) with [`norlab_icp_mapper`](https://github.com/norlab-ulaval/norlab_icp_mapper) for simultaneous localization and mapping and [`GeRoNa`](https://github.com/cogsys-tuebingen/gerona) for path-following control.
 12 | 
 13 | ## Installation
 14 | 
 15 | ### Setting up libpointmatcher and the mapper
 16 | 
 17 | First, in your local repositories directory, clone and build `norlab_icp_mapper`:
 18 | 
 19 | ```sh
 20 | cd && cd repos/
 21 | git clone git@github.com:ethz-asl/libpointmatcher.git  && cd libpointmatcher/
 22 | mkdir build/ && cd build/
 23 | cmake -DCMAKE_BUILD_TYPE=Release ..
 24 | make -j 6
 25 | sudo make install
 26 | cd && cd repos
 27 | git clone git@github.com:norlab-ulaval/norlab_icp_mapper.git && cd norlab_icp_mapper/
 28 | mkdir build/ && cd build/
 29 | cmake -DCMAKE_BUILD_TYPE=Release ..
 30 | make -j 6
 31 | sudo make install
 32 | ```
 33 | 
 34 | Then, in your ROS catkin workspace, add the ROS packages to perform SLAM
 35 | 
 36 | ```sh
 37 | cd && cd catkin_ws/src
 38 | git clone git@github.com:norlab-ulaval/norlab_icp_mapper_ros.git
 39 | git clone git@github.com:norlab-ulaval/libpointmatcher_ros.git
 40 | git clone git@github.com:norlab-ulaval/pointcloud_motion_deskew.git
 41 | git clone git@github.com:norlab-ulaval/odom_to_pose_converter.git
 42 | cd .. && catkin_make
 43 | ```
 44 | 
 45 | ### Setting up GeRoNa
 46 | 
 47 | GeRoNa is a collection of ROS packages, which can be installed along with their dependencies this way:
 48 | 
 49 | First, install the dependencies. For example, from your workspace root directory:
 50 | 
 51 | ```sh
 52 | cd  && cd catkin_ws/src
 53 | git clone https://github.com/cogsys-tuebingen/cslibs_path_planning
 54 | git clone https://github.com/cogsys-tuebingen/cslibs_navigation_utilities
 55 | sudo apt install libalglib-dev
 56 | git clone https://github.com/cogsys-tuebingen/gerona.git
 57 | cd ..
 58 | 
 59 | rosdep install --from-paths -i -r -y src
 60 | catkin_make
 61 | ```
 62 | 
 63 | ## Quick start
 64 | 
 65 | General note : This assumes that you already have standard [timestamped lidar scan](https://github.com/norlab-ulaval/ros_rslidar), [imu](https://github.com/ethz-asl/ethzasl_xsens_driver) and general odometry nodes running.
 66 | 
 67 | ### Creating a params launch file
 68 | 
 69 | Two distinct configuration files are required. The first ones are intended to define the path following parameters, multiple examples can be found in the [`params/controller`](https://github.com/norlab-ulaval/WILN/tree/master/params/controller) directory. For an extensive definition of parameters, please refer to the [GeRoNa wiki](https://github.com/cogsys-tuebingen/gerona/wiki/path-follower-parameters).
 70 | 
 71 | The second ones define the registration parameters and filters for the SLAM algorithm. A working example can be found in the [`params/icp`](params/) directory. For more details on mapping parameters, please refer to the [norlab_icp_mapper_ros](https://github.com/norlab-ulaval/norlab_icp_mapper_ros) repository.
 72 | 
 73 | ### Creating a general launch file
 74 | 
 75 | General launch files will launch the various nodes required for navigation. They will also import the aforementioned parameters. A [launch file example](https://github.com/norlab-ulaval/WILN/blob/master/launch/warthog.launch) is given.
 76 | 
 77 | ## General operation and services
 78 | 
 79 | The framework is divided into two phases: teach and repeat. During the teach phase, an operator drives the robot along a desired route. The robot simultaneously localizes and builds a map of the environment. All robot poses are logged and represent the reference trajectory.
 80 | 
 81 | During the repeat phase, the robot repeats a given route. This route can either be a map that has just been recorder or a loaded map that was saved on disk. During this phase, the system registers point clouds to the map to localize but does not update the map.
 82 | 
 83 | The following table lists the various ROS services that enable the teach-and-repeat framework:
 84 | 
 85 | | Service name | Description | Parameters |
 86 | | :----------- | :---------- | :--------- |
 87 | | /start_recording | Starts recording poses to build the reference map (cannot be called if another trajectory is already loaded). | None |
 88 | | /stop_recording | Stops the trajectory recording (cannot be called is the recording was not started). | None                 |
 89 | | /play_line | Starts the repeat phase. The robot will repeat the trajectory backwards if it is located at it's end and the system supports both forwards or reverse motion. | None |
 90 | | /play_loop | Starts the repeat phase for a loop trajectory. | nb_loops (uint32)     |
 91 | | /cancel_trajectory | Cancels the current repeat phase in the event of system failure. | None  |
 92 | | /clear_trajectory | Clears the current trajectory from active memory. | None  |
 93 | | /smooth_trajectory | Smooths the current trajectory using a low-pass filter. | None  |
 94 | | /flip_trajectory | Flips the orientations of all the poses in the trajectory, without altering their order. | None  |
 95 | | /reverse_trajectory | Reverses the order of the poses in the trajectory, without altering their orientation. | None  |
 96 | | /save_map_traj | Saves the current map and trajectory in a `.ltr` file. | `file_name` (string) |
 97 | | /load_map_traj | loads a specicied `.ltr` file. Assumes the robot is at the start of the trajectory. | `file_name` (string) |
 98 | | /load_map_traj_from_end | loads a specicied `.ltr` file. Assumes the robot is at the en of the trajectory. | `file_name` (string) |
 99 | 
100 | ## Citing
101 | 
102 | If you use wiln in an academic context, please cite [our article](https://doi.org/10.55417/fr.2022050):
103 | 
104 | ```bibtex
105 | @article{Baril2022,
106 |   doi = {10.55417/fr.2022050},
107 |   url = {https://doi.org/10.55417/fr.2022050},
108 |   year = {2022},
109 |   month = mar,
110 |   publisher = {Field Robotics Publication Society},
111 |   volume = {2},
112 |   number = {1},
113 |   pages = {1628--1660},
114 |   author = {Dominic Baril and Simon-Pierre Desch{\^{e}}nes and Olivier Gamache and Maxime Vaidis and Damien LaRocque and Johann Laconte and Vladim{\'{\i}}r Kubelka and Philippe Gigu{\`{e}}re and Fran{\c{c}}ois Pomerleau},
115 |   title = {Kilometer-scale autonomous navigation in subarctic forests: challenges and lessons learned},
116 |   journal = {Field Robotics}
117 | }
118 | ```
119 | 


--------------------------------------------------------------------------------
/trajectories/square.ltr:
--------------------------------------------------------------------------------
  1 | #############################
  2 | frame_id : base_link
  3 | 0.0,0.0,0.0,0.0,0.0,0.0,1.0
  4 | 0.12012012225063831,0.0,0.0,0.0,0.0,0.0,1.0
  5 | 0.24024024450127662,0.0,0.0,0.0,0.0,0.0,1.0
  6 | 0.36036036675191496,0.0,0.0,0.0,0.0,0.0,1.0
  7 | 0.48048048900255325,0.0,0.0,0.0,0.0,0.0,1.0
  8 | 0.6006006112531941,0.0,0.0,0.0,0.0,0.0,1.0
  9 | 0.7207207335038324,0.0,0.0,0.0,0.0,0.0,1.0
 10 | 0.8408408557544707,0.0,0.0,0.0,0.0,0.0,1.0
 11 | 0.960960978005109,0.0,0.0,0.0,0.0,0.0,1.0
 12 | 1.0810811002557472,0.0,0.0,0.0,0.0,0.0,1.0
 13 | 1.2012012225063857,0.0,0.0,0.0,0.0,0.0,1.0
 14 | 1.321321344757024,0.0,0.0,0.0,0.0,0.0,1.0
 15 | 1.4414414670076623,0.0,0.0,0.0,0.0,0.0,1.0
 16 | 1.5615615892583006,0.0,0.0,0.0,0.0,0.0,1.0
 17 | 1.6816817115089413,0.0,0.0,0.0,0.0,0.0,1.0
 18 | 1.8018018337595798,0.0,0.0,0.0,0.0,0.0,1.0
 19 | 1.921921956010218,0.0,0.0,0.0,0.0,0.0,1.0
 20 | 2.0420420782608564,0.0,0.0,0.0,0.0,0.0,1.0
 21 | 2.1621622005114944,0.0,0.0,0.0,0.0,0.0,1.0
 22 | 2.282282322762133,0.0,0.0,0.0,0.0,0.0,1.0
 23 | 2.4024024450127714,0.0,0.0,0.0,0.0,0.0,1.0
 24 | 2.5225225672634095,0.0,0.0,0.0,0.0,0.0,1.0
 25 | 2.642642689514048,0.0,0.0,0.0,0.0,0.0,1.0
 26 | 2.7627628117646887,0.0,0.0,0.0,0.0,0.0,1.0
 27 | 2.8828829340153272,0.0,0.0,0.0,0.0,0.0,1.0
 28 | 3.0030030562659653,0.0,0.0,0.0,0.0,0.0,1.0
 29 | 3.123123178516604,0.0,0.0,0.0,0.0,0.0,1.0
 30 | 3.243243300767242,0.0,0.0,0.0,0.0,0.0,1.0
 31 | 3.3633634230178804,0.0,0.0,0.0,0.0,0.0,1.0
 32 | 3.4834835452685184,0.0,0.0,0.0,0.0,0.0,1.0
 33 | 3.603603667519157,0.0,0.0,0.0,0.0,0.0,1.0
 34 | 3.723723789769795,0.0,0.0,0.0,0.0,0.0,1.0
 35 | 3.843843912020436,0.0,0.0,0.0,0.0,0.0,1.0
 36 | 3.9639640342710742,0.0,0.0,0.0,0.0,0.0,1.0
 37 | 4.084084156521713,0.0,0.0,0.0,0.0,0.0,1.0
 38 | 4.204204278772351,0.0,0.0,0.0,0.0,0.0,1.0
 39 | 4.324324401022989,0.0,0.0,0.0,0.0,0.0,1.0
 40 | 4.444444523273628,0.0,0.0,0.0,0.0,0.0,1.0
 41 | 4.564564645524266,0.0,0.0,0.0,0.0,0.0,1.0
 42 | 4.684684767774904,0.0,0.0,0.0,0.0,0.0,1.0
 43 | 4.804804890025543,0.0,0.0,0.0,0.0,0.0,1.0
 44 | 4.924925012276184,0.0,0.0,0.0,0.0,0.0,1.0
 45 | 5.045045134526819,0.0,0.0,0.0,0.0,0.0,1.0
 46 | 5.16516525677746,0.0,0.0,0.0,0.0,0.0,1.0
 47 | 5.285285379028099,0.0,0.0,0.0,0.0,0.0,1.0
 48 | 5.405405501278737,0.0,0.0,0.0,0.0,0.0,1.0
 49 | 5.525525623529375,0.0,0.0,0.0,0.0,0.0,1.0
 50 | 5.645645745780013,0.0,0.0,0.0,0.0,0.0,1.0
 51 | 5.7657658680306545,0.0,0.0,0.0,0.0,0.0,1.0
 52 | 5.88588599028129,0.0,0.0,0.0,0.0,0.0,1.0
 53 | 6.006006112531931,0.0,0.0,0.0,0.0,0.0,1.0
 54 | 6.126126234782567,0.0,0.0,0.0,0.0,0.0,1.0
 55 | 6.246246357033208,0.0,0.0,0.0,0.0,0.0,1.0
 56 | 6.366366479283846,0.0,0.0,0.0,0.0,0.0,1.0
 57 | 6.486486601534484,0.0,0.0,0.0,0.0,0.0,1.0
 58 | 6.6066067237851245,0.0,0.0,0.0,0.0,0.0,1.0
 59 | 6.726726846035761,0.0,0.0,0.0,0.0,0.0,1.0
 60 | 6.8468469682864015,0.0,0.0,0.0,0.0,0.0,1.0
 61 | 6.966967090537037,0.0,0.0,0.0,0.0,0.0,1.0
 62 | 7.087087212787678,0.0,0.0,0.0,0.0,0.0,1.0
 63 | 7.207207335038314,0.0,0.0,0.0,0.0,0.0,1.0
 64 | 7.327327457288955,0.0,0.0,0.0,0.0,0.0,1.0
 65 | 7.447447579539593,0.0,0.0,0.0,0.0,0.0,1.0
 66 | 7.567567701790232,0.0,0.0,0.0,0.0,0.0,1.0
 67 | 7.687687824040872,0.0,0.0,0.0,0.0,0.0,1.0
 68 | 7.807807946291508,0.0,0.0,0.0,0.0,0.0,1.0
 69 | 7.9279280685421485,0.0,0.0,0.0,0.0,0.0,1.0
 70 | 8.048048190792784,0.0,0.0,0.0,0.0,0.0,1.0
 71 | 8.168168313043425,0.0,0.0,0.0,0.0,0.0,1.0
 72 | 8.288288435294062,0.0,0.0,0.0,0.0,0.0,1.0
 73 | 8.408408557544702,0.0,0.0,0.0,0.0,0.0,1.0
 74 | 8.52852867979534,0.0,0.0,0.0,0.0,0.0,1.0
 75 | 8.648648802045978,0.0,0.0,0.0,0.0,0.0,1.0
 76 | 8.76876892429662,0.0,0.0,0.0,0.0,0.0,1.0
 77 | 8.888889046547256,0.0,0.0,0.0,0.0,0.0,1.0
 78 | 9.009009168797895,0.0,0.0,0.0,0.0,0.0,1.0
 79 | 9.129129291048534,0.0,0.0,0.0,0.0,0.0,1.0
 80 | 9.249249413299173,0.0,0.0,0.0,0.0,0.0,1.0
 81 | 9.369369535549808,0.0,0.0,0.0,0.0,0.0,1.0
 82 | 9.48948965780045,0.0,0.0,0.0,0.0,0.0,1.0
 83 | 9.609609780051088,0.0,0.0,0.0,0.0,0.0,1.0
 84 | 9.729729902301726,0.0,0.0,0.0,0.0,0.0,1.0
 85 | 9.849850024552367,0.0,0.0,0.0,0.0,0.0,1.0
 86 | 9.969970146803002,0.0,0.0,0.0,0.0,0.0,1.0
 87 | 10.0,0.13013030980385426,0.0,0.0,0.0,0.6225520240174707,0.7825784161294961
 88 | 10.0,0.2502504320544951,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 89 | 10.0,0.3703705543051309,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 90 | 10.0,0.4904906765557717,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 91 | 10.0,0.6106107988064126,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 92 | 10.0,0.7307309210570484,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 93 | 10.0,0.8508510433076892,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 94 | 10.0,0.970971165558325,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 95 | 10.0,1.0910912878089658,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 96 | 10.0,1.2112114100596065,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 97 | 10.0,1.3313315323102424,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 98 | 10.0,1.4514516545608782,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
 99 | 10.0,1.5715717768115192,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
100 | 10.0,1.6916918990621599,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
101 | 10.0,1.8118120213127957,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
102 | 10.0,1.9319321435634365,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
103 | 10.0,2.0520522658140723,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
104 | 10.0,2.172172388064713,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
105 | 10.0,2.2922925103153537,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
106 | 10.0,2.4124126325659896,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
107 | 10.0,2.5325327548166308,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
108 | 10.0,2.652652877067266,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
109 | 10.0,2.7727729993179073,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
110 | 10.0,2.892893121568543,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
111 | 10.0,3.013013243819184,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
112 | 10.0,3.1331333660698197,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
113 | 10.0,3.2532534883204605,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
114 | 10.0,3.373373610571101,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
115 | 10.0,3.493493732821737,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
116 | 10.0,3.613613855072373,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
117 | 10.0,3.7337339773230136,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
118 | 10.0,3.8538540995736543,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
119 | 10.0,3.97397422182429,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
120 | 10.0,4.094094344074931,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
121 | 10.0,4.214214466325567,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
122 | 10.0,4.3343345885762075,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
123 | 10.0,4.454454710826848,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
124 | 10.0,4.5745748330774845,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
125 | 10.0,4.694694955328125,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
126 | 10.0,4.814815077578761,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
127 | 10.0,4.934935199829402,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
128 | 10.0,5.055055322080043,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
129 | 10.0,5.175175444330678,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
130 | 10.0,5.295295566581315,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
131 | 10.0,5.415415688831955,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
132 | 10.0,5.535535811082596,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
133 | 10.0,5.6556559333332315,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
134 | 10.0,5.775776055583868,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
135 | 10.0,5.8958961778345085,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
136 | 10.0,6.016016300085149,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
137 | 10.0,6.136136422335785,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
138 | 10.0,6.256256544586426,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
139 | 10.0,6.376376666837067,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
140 | 10.0,6.496496789087702,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
141 | 10.0,6.616616911338343,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
142 | 10.0,6.736737033588984,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
143 | 10.0,6.85685715583962,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
144 | 10.0,6.9769772780902555,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
145 | 10.0,7.097097400340896,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
146 | 10.0,7.217217522591537,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
147 | 10.0,7.337337644842173,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
148 | 10.0,7.457457767092809,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
149 | 10.0,7.577577889343449,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
150 | 10.0,7.697698011594091,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
151 | 10.0,7.817818133844726,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
152 | 10.0,7.937938256095367,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
153 | 10.0,8.058058378346002,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
154 | 10.0,8.178178500596644,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
155 | 10.0,8.298298622847284,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
156 | 10.0,8.41841874509792,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
157 | 10.0,8.538538867348562,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
158 | 10.0,8.658658989599196,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
159 | 10.0,8.778779111849838,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
160 | 10.0,8.89889923410048,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
161 | 10.0,9.019019356351114,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
162 | 10.0,9.13913947860175,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
163 | 10.0,9.259259600852392,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
164 | 10.0,9.379379723103032,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
165 | 10.0,9.499499845353668,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
166 | 10.0,9.619619967604308,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
167 | 10.0,9.739740089854944,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
168 | 10.0,9.859860212105586,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
169 | 10.0,9.97998033435622,0.0,0.0,0.0,0.7071067811865475,0.7071067811865476
170 | 9.899899898124467,10.000000354731329,0.0,0.0,0.0,0.9951333266680693,0.09853761796665068
171 | 9.779779775873829,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
172 | 9.659659653623192,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
173 | 9.53953953137255,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
174 | 9.419419409121916,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
175 | 9.299299286871275,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
176 | 9.179179164620633,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
177 | 9.059059042369999,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
178 | 8.938938920119357,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
179 | 8.81881879786872,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
180 | 8.698698675618083,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
181 | 8.578578553367443,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
182 | 8.458458431116807,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
183 | 8.338338308866165,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
184 | 8.21821818661553,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
185 | 8.098098064364889,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
186 | 7.977977942114248,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
187 | 7.857857819863613,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
188 | 7.737737697612972,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
189 | 7.617617575362333,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
190 | 7.497497453111695,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
191 | 7.377377330861057,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
192 | 7.257257208610421,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
193 | 7.13713708635978,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
194 | 7.017016964109139,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
195 | 6.896896841858504,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
196 | 6.776776719607862,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
197 | 6.656656597357227,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
198 | 6.536536475106589,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
199 | 6.416416352855948,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
200 | 6.296296230605312,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
201 | 6.176176108354671,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
202 | 6.0560559861040355,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
203 | 5.935935863853394,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
204 | 5.815815741602753,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
205 | 5.695695619352118,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
206 | 5.575575497101477,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
207 | 5.455455374850836,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
208 | 5.3353352526002,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
209 | 5.215215130349562,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
210 | 5.095095008098927,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
211 | 4.974974885848285,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
212 | 4.854854763597644,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
213 | 4.734734641347009,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
214 | 4.614614519096368,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
215 | 4.494494396845732,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
216 | 4.374374274595094,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
217 | 4.254254152344453,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
218 | 4.134134030093817,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
219 | 4.014013907843176,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
220 | 3.8938937855925406,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
221 | 3.7737736633418995,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
222 | 3.6536535410912587,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
223 | 3.533533418840623,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
224 | 3.413413296589982,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
225 | 3.2932931743393463,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
226 | 3.1731730520887056,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
227 | 3.053052929838067,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
228 | 2.932932807587429,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
229 | 2.8128126853367905,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
230 | 2.69269256308615,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
231 | 2.572572440835514,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
232 | 2.452452318584876,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
233 | 2.3323321963342374,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
234 | 2.2122120740835993,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
235 | 2.092091951832958,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
236 | 1.9719718295823225,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
237 | 1.8518517073316816,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
238 | 1.731731585081046,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
239 | 1.611611462830405,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
240 | 1.4914913405797643,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
241 | 1.3713712183291284,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
242 | 1.2512510960784875,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
243 | 1.1311309738278519,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
244 | 1.011010851577211,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
245 | 0.8908907293265727,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
246 | 0.7707706070759344,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
247 | 0.650650484825296,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
248 | 0.5305303625746551,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
249 | 0.4104102403240194,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
250 | 0.29029011807338106,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
251 | 0.17016999582274275,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
252 | 0.050049873572104456,10.000000354731329,0.0,0.0,0.0,1.0,6.123233995736766e-17
253 | 0.0,9.889890065302577,0.0,0.0,0.0,-0.8407738032793362,0.5413865640364565
254 | 0.0,9.76976994305194,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
255 | 0.0,9.649649820801306,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
256 | 0.0,9.529529698550665,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
257 | 0.0,9.409409576300028,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
258 | 0.0,9.289289454049388,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
259 | 0.0,9.169169331798752,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
260 | 0.0,9.04904920954811,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
261 | 0.0,8.92892908729747,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
262 | 0.0,8.808808965046834,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
263 | 0.0,8.688688842796195,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
264 | 0.0,8.568568720545558,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
265 | 0.0,8.448448598294917,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
266 | 0.0,8.328328476044277,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
267 | 0.0,8.20820835379364,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
268 | 0.0,8.088088231542999,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
269 | 0.0,7.967968109292364,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
270 | 0.0,7.847847987041723,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
271 | 0.0,7.727727864791082,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
272 | 0.0,7.607607742540447,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
273 | 0.0,7.487487620289806,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
274 | 0.0,7.36736749803917,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
275 | 0.0,7.247247375788529,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
276 | 0.0,7.127127253537888,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
277 | 0.0,7.007007131287253,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
278 | 0.0,6.8868870090366165,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
279 | 0.0,6.766766886785976,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
280 | 0.0,6.64664676453534,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
281 | 0.0,6.526526642284699,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
282 | 0.0,6.406406520034063,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
283 | 0.0,6.286286397783423,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
284 | 0.0,6.166166275532782,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
285 | 0.0,6.046046153282146,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
286 | 0.0,5.925926031031505,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
287 | 0.0,5.8058059087808696,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
288 | 0.0,5.685685786530229,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
289 | 0.0,5.565565664279588,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
290 | 0.0,5.445445542028952,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
291 | 0.0,5.325325419778311,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
292 | 0.0,5.20520529752767,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
293 | 0.0,5.085085175277034,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
294 | 0.0,4.964965053026393,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
295 | 0.0,4.844844930775758,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
296 | 0.0,4.724724808525122,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
297 | 0.0,4.604604686274481,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
298 | 0.0,4.48448456402384,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
299 | 0.0,4.364364441773205,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
300 | 0.0,4.2442443195225685,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
301 | 0.0,4.124124197271923,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
302 | 0.0,4.004004075021287,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
303 | 0.0,3.8838839527706512,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
304 | 0.0,3.7637638305200105,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
305 | 0.0,3.6436437082693747,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
306 | 0.0,3.523523586018734,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
307 | 0.0,3.403403463768093,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
308 | 0.0,3.2832833415174574,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
309 | 0.0,3.1631632192668167,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
310 | 0.0,3.043043097016181,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
311 | 0.0,2.9229229747655396,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
312 | 0.0,2.802802852514899,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
313 | 0.0,2.682682730264263,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
314 | 0.0,2.5625626080136223,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
315 | 0.0,2.4424424857629816,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
316 | 0.0,2.3223223635123458,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
317 | 0.0,2.20220224126171,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
318 | 0.0,2.082082119011074,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
319 | 0.0,1.9619619967604283,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
320 | 0.0,1.8418418745097924,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
321 | 0.0,1.7217217522591568,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
322 | 0.0,1.6016016300085159,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
323 | 0.0,1.48148150775788,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
324 | 0.0,1.3613613855072393,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
325 | 0.0,1.2412412632565983,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
326 | 0.0,1.1211211410059627,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
327 | 0.0,1.0010010187553218,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
328 | 0.0,0.880880896504686,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
329 | 0.0,0.7607607742540452,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
330 | 0.0,0.6406406520034044,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
331 | 0.0,0.5205205297527685,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
332 | 0.0,0.4004004075021277,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
333 | 0.0,0.28028028525148685,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
334 | 0.0,0.1601601630008511,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476
335 | 0.0,0.04004004075021025,0.0,0.0,0.0,-0.7071067811865475,0.7071067811865476


--------------------------------------------------------------------------------
/src/wiln.cpp:
--------------------------------------------------------------------------------
  1 | #include <rclcpp/rclcpp.hpp>
  2 | #include <fstream>
  3 | #include <mutex>
  4 | #include "wiln.hpp"
  5 | #include "utils.hpp"
  6 | #include <sys/stat.h>
  7 | 
  8 | using namespace std::chrono_literals;
  9 | 
 10 | WilnNode::WilnNode() : Node("wiln_node"), currentState(State::IDLE)
 11 | {
 12 |     initParameters();
 13 |     initSubscribers();
 14 |     initPublishers();
 15 |     initServices();
 16 |     initClients();
 17 |     initTimers();
 18 | }
 19 | 
 20 | void WilnNode::initParameters()
 21 | {
 22 |     this->declare_parameter("odom_topic", "odom");
 23 |     this->declare_parameter("distance_between_waypoints", 0.05);
 24 |     this->declare_parameter("angle_between_waypoints", 0.1);
 25 |     this->declare_parameter("trajectory_speed", 1.5);
 26 |     this->declare_parameter("smoothing_window_size", 9);
 27 |     this->declare_parameter("follow_path_topic", "/follow_path");
 28 |     this->declare_parameter("loop_closure_linear_tolerance", 5.0);
 29 |     this->declare_parameter("loop_closure_angular_tolerance", M_PI / 4);
 30 |     updateParameters();
 31 | }
 32 | 
 33 | void WilnNode::updateParameters()
 34 | {
 35 |     this->get_parameter("odom_topic", odomTopic);
 36 |     this->get_parameter("distance_between_waypoints", distanceBetweenWaypoints);
 37 |     this->get_parameter("angle_between_waypoints", angleBetweenWaypoints);
 38 |     this->get_parameter("trajectory_speed", trajectorySpeed);
 39 |     this->get_parameter("smoothing_window_size", smoothingWindowSize);
 40 |     this->get_parameter("follow_path_topic", followPathTopic);
 41 |     this->get_parameter("loop_closure_linear_tolerance", loopClosureLinearTolerance);
 42 |     this->get_parameter("loop_closure_angular_tolerance", loopClosureAngularTolerance);
 43 | }
 44 | 
 45 | void WilnNode::initSubscribers()
 46 | {
 47 |     odomSubscription = this->create_subscription<nav_msgs::msg::Odometry>(
 48 |         odomTopic, 10, std::bind(&WilnNode::odomCallback, this, std::placeholders::_1)
 49 |     );
 50 |     // trajectoryResultSubscription = this->create_subscription<controller_msgs::action::FollowPath::Result>(
 51 |     //         "follow_path/result", 1000, std::bind(&WilnNode::trajectoryResultCallback, this, std::placeholders::_1)
 52 |     // );
 53 | }
 54 | 
 55 | void WilnNode::initPublishers()
 56 | {
 57 |     auto publisher_qos = rclcpp::QoS(10);
 58 |     publisher_qos.transient_local();
 59 |     plannedTrajectoryPublisher = this->create_publisher<nav_msgs::msg::Path>("planned_trajectory", publisher_qos);
 60 |     realTrajectoryPublisher = this->create_publisher<nav_msgs::msg::Path>("real_trajectory", publisher_qos);
 61 |     statePublisher = this->create_publisher<std_msgs::msg::UInt8>("state", 1);
 62 | 
 63 |     publishPlannedTrajectory();
 64 |     publishRealTrajectory();
 65 | }
 66 | 
 67 | void WilnNode::initServices()
 68 | {
 69 |     startRecordingService = this->create_service<std_srvs::srv::Empty>(
 70 |         "start_recording", std::bind(&WilnNode::startRecordingServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 71 |     );
 72 |     stopRecordingService = this->create_service<std_srvs::srv::Empty>(
 73 |         "stop_recording", std::bind(&WilnNode::stopRecordingServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 74 |     );
 75 |     saveMapTrajService = this->create_service<wiln_msgs::srv::SaveMapTraj>(
 76 |         "save_map_traj", std::bind(&WilnNode::saveLTRServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 77 |     );
 78 |     loadMapTrajService = this->create_service<wiln_msgs::srv::LoadMapTraj>(
 79 |         "load_map_traj", std::bind(&WilnNode::loadLTRServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 80 |     );
 81 |     loadMapTrajFromEndService = this->create_service<wiln_msgs::srv::LoadMapTraj>(
 82 |         "load_map_traj_from_end", std::bind(&WilnNode::loadLTRFromEndServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 83 |     );
 84 |     playLoopService = this->create_service<wiln_msgs::srv::PlayLoop>(
 85 |         "play_loop", std::bind(&WilnNode::playLoopServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 86 |     );
 87 |     playLineService = this->create_service<std_srvs::srv::Empty>(
 88 |         "play_line", std::bind(&WilnNode::playLineServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 89 |     );
 90 |     cancelTrajectoryService = this->create_service<std_srvs::srv::Empty>(
 91 |         "cancel_trajectory", std::bind(&WilnNode::cancelTrajectoryServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 92 |     );
 93 |     smoothTrajectoryService = this->create_service<std_srvs::srv::Empty>(
 94 |         "smooth_trajectory", std::bind(&WilnNode::smoothTrajectoryServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 95 |     );
 96 |     clearTrajectoryService = this->create_service<std_srvs::srv::Empty>(
 97 |         "clear_trajectory", std::bind(&WilnNode::clearTrajectoryServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
 98 |     );
 99 |     reverseTrajectoryService = this->create_service<std_srvs::srv::Empty>(
100 |         "reverse_trajectory", std::bind(&WilnNode::reverseTrajectoryServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
101 |     );
102 |     flipTrajectoryService = this->create_service<std_srvs::srv::Empty>(
103 |         "flip_trajectory", std::bind(&WilnNode::flipTrajectoryServiceCallback, this, std::placeholders::_1, std::placeholders::_2)
104 |     );
105 | }
106 | 
107 | void WilnNode::initClients()
108 | {
109 |     enableMappingClient = this->create_client<std_srvs::srv::Empty>("/mapping/enable_mapping");
110 |     disableMappingClient = this->create_client<std_srvs::srv::Empty>("/mapping/disable_mapping");
111 |     saveMapClient = this->create_client<norlab_icp_mapper_ros::srv::SaveMap>("/mapping/save_map");
112 |     loadMapClient = this->create_client<norlab_icp_mapper_ros::srv::LoadMap>("/mapping/load_map");
113 |     followPathClient = rclcpp_action::create_client<controller_msgs::action::FollowPath>(this, followPathTopic);
114 | }
115 | 
116 | void WilnNode::initTimers()
117 | {
118 |     stateTimer = this->create_wall_timer(500ms, std::bind(&WilnNode::publishState, this));
119 |     updateTimer = this->create_wall_timer(2s, std::bind(&WilnNode::updateParameters, this));
120 | }
121 | 
122 | void WilnNode::odomCallback(const nav_msgs::msg::Odometry &odomMsg)
123 | {
124 |     robotPoseLock.lock();
125 |     robotPose.header = odomMsg.header;
126 |     robotPose.pose = odomMsg.pose.pose;
127 |     robotPoseLock.unlock();
128 | 
129 |     if (currentState == State::RECORDING)
130 |     {
131 |         updatePlannedTrajectory(robotPose);
132 |     }
133 |     else if (currentState == State::PLAYING)
134 |     {
135 |         updateRealTrajectory(robotPose);
136 |     }
137 | }
138 | 
139 | void WilnNode::updatePlannedTrajectory(const geometry_msgs::msg::PoseStamped &currentPose)
140 | {
141 |     // Create new trajectory if none exists
142 |     if (plannedTrajectory.poses.empty())
143 |     {
144 |         plannedTrajectory.header.frame_id = currentPose.header.frame_id;
145 |         plannedTrajectory.header.stamp = this->now();
146 |         plannedTrajectory.poses.push_back(currentPose);
147 |     }
148 |     else
149 |     {
150 |         // Add points if the robot moved enough
151 |         geometry_msgs::msg::Pose lastPose = plannedTrajectory.poses.back().pose;
152 |         auto [diff_lin, diff_ang] = diffBetweenPoses(lastPose, currentPose.pose);
153 |         if (diff_lin >= distanceBetweenWaypoints || std::fabs(diff_ang) > angleBetweenWaypoints)
154 |         {
155 |             plannedTrajectory.poses.push_back(currentPose);
156 |         }
157 |     }
158 | 
159 |     publishPlannedTrajectory();
160 | }
161 | 
162 | void WilnNode::updateRealTrajectory(const geometry_msgs::msg::PoseStamped &currentPose)
163 | {
164 |     // Create new trajectory if none exists
165 |     if (realTrajectory.poses.empty())
166 |     {
167 |         realTrajectory.header.frame_id = currentPose.header.frame_id;
168 |         realTrajectory.header.stamp = this->now();
169 |         realTrajectory.poses.push_back(currentPose);
170 |     }
171 |     else
172 |     {
173 |         // Add points if the robot moved enough
174 |         geometry_msgs::msg::Pose lastPose = realTrajectory.poses.back().pose;
175 |         auto [diff_lin, diff_ang] = diffBetweenPoses(lastPose, currentPose.pose);
176 |         if (diff_lin >= distanceBetweenWaypoints || std::fabs(diff_ang) > angleBetweenWaypoints)
177 |         {
178 |             realTrajectory.poses.push_back(currentPose);
179 |         }
180 |     }
181 | 
182 |     publishRealTrajectory();
183 | }
184 | 
185 | void WilnNode::startRecordingServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
186 | {
187 |     switch (currentState)
188 |     {
189 |         case State::IDLE:
190 |         {
191 |             currentState = State::RECORDING;
192 |             auto enableMappingRequest = std::make_shared<std_srvs::srv::Empty::Request>();
193 |             enableMappingClient->async_send_request(enableMappingRequest);
194 |             RCLCPP_INFO(this->get_logger(), "Recording started.");
195 |             break;
196 |         }
197 |         case State::RECORDING:
198 |         {
199 |             RCLCPP_WARN(this->get_logger(), "Trajectory is already being recorded.");
200 |             break;
201 |         }
202 |         case State::PLAYING:
203 |         {
204 |             RCLCPP_WARN(this->get_logger(), "Cannot start recording, trajectory is currently being played.");
205 |             break;
206 |         }
207 |     }
208 | }
209 | 
210 | void WilnNode::stopRecordingServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
211 | {
212 |     switch (currentState)
213 |     {
214 |         case State::IDLE:
215 |         {
216 |             RCLCPP_WARN(this->get_logger(), "Cannot stop recording, no trajectory is being recorded.");
217 |             break;
218 |         }
219 |         case State::RECORDING:
220 |         {
221 |             currentState = State::IDLE;
222 |             auto disableMappingRequest = std::make_shared<std_srvs::srv::Empty::Request>();
223 |             disableMappingClient->async_send_request(disableMappingRequest);
224 |             RCLCPP_INFO(this->get_logger(), "Recording stopped.");
225 |             break;
226 |         }
227 |         case State::PLAYING:
228 |         {
229 |             RCLCPP_WARN(this->get_logger(), "Cannot stop recording, trajectory is currently being played.");
230 |             break;
231 |         }
232 |     }
233 | }
234 | 
235 | void WilnNode::clearTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
236 | {
237 |     switch (currentState)
238 |     {   
239 |         case State::IDLE:
240 |         {
241 |             RCLCPP_INFO(this->get_logger(), "Clearing trajectory...");
242 |             plannedTrajectory.poses.clear();
243 |             realTrajectory.poses.clear();
244 |             publishPlannedTrajectory();
245 |             publishRealTrajectory();
246 |             RCLCPP_INFO(this->get_logger(), "Done.");
247 |             break;
248 |         }
249 |         case State::RECORDING:
250 |         {
251 |             RCLCPP_WARN(this->get_logger(), "Cannot clear trajectory while recording.");
252 |             break;
253 |         }
254 |         case State::PLAYING:
255 |         {
256 |             RCLCPP_WARN(this->get_logger(), "Cannot clear trajectory while playing.");
257 |             break;
258 |         }
259 |     }
260 | }
261 | 
262 | void WilnNode::reverseTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
263 | {
264 |     switch (currentState)
265 |     {   
266 |         case State::IDLE:
267 |         {
268 |             RCLCPP_INFO(this->get_logger(), "Reversing trajectory...");
269 |             plannedTrajectory = reversePath(plannedTrajectory);
270 |             publishPlannedTrajectory();
271 |             RCLCPP_INFO(this->get_logger(), "Done.");
272 |             break;
273 |         }
274 |         case State::RECORDING:
275 |         {
276 |             RCLCPP_WARN(this->get_logger(), "Cannot reverse trajectory while recording.");
277 |             break;
278 |         }
279 |         case State::PLAYING:
280 |         {
281 |             RCLCPP_WARN(this->get_logger(), "Cannot reverse trajectory while playing.");
282 |             break;
283 |         }
284 |     }
285 | }
286 | 
287 | void WilnNode::flipTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
288 | {
289 |     switch (currentState)
290 |     {   
291 |         case State::IDLE:
292 |         {
293 |             RCLCPP_INFO(this->get_logger(), "Flipping trajectory...");
294 |             plannedTrajectory = flipPath(plannedTrajectory);
295 |             publishPlannedTrajectory();
296 |             RCLCPP_INFO(this->get_logger(), "Done.");
297 |             break;
298 |         }
299 |         case State::RECORDING:
300 |         {
301 |             RCLCPP_WARN(this->get_logger(), "Cannot flip trajectory while recording.");
302 |             break;
303 |         }
304 |         case State::PLAYING:
305 |         {
306 |             RCLCPP_WARN(this->get_logger(), "Cannot flip trajectory while playing.");
307 |             break;
308 |         }
309 |     }
310 | }
311 | 
312 | void WilnNode::smoothTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
313 | {
314 |     switch (currentState)
315 |     {   
316 |         case State::IDLE:
317 |         {
318 |             RCLCPP_INFO(this->get_logger(), "Smoothing trajectory...");
319 |             plannedTrajectory = smoothPathLowPass(plannedTrajectory, smoothingWindowSize);
320 |             publishPlannedTrajectory();
321 |             RCLCPP_INFO(this->get_logger(), "Done.");
322 |             break;
323 |         }
324 |         case State::RECORDING:
325 |         {
326 |             RCLCPP_WARN(this->get_logger(), "Cannot smooth trajectory while recording.");
327 |             break;
328 |         }
329 |         case State::PLAYING:
330 |         {
331 |             RCLCPP_WARN(this->get_logger(), "Cannot smooth trajectory while playing.");
332 |             break;
333 |         }
334 |     }
335 |     
336 | }
337 | 
338 | void WilnNode::cancelTrajectoryServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
339 | {
340 |     switch (currentState)
341 |     {   
342 |         case State::IDLE:
343 |         {
344 |             RCLCPP_WARN(this->get_logger(), "Cannot cancel trajectory, no trajectory is being played.");
345 |             break;
346 |         }
347 |         case State::RECORDING:
348 |         {
349 |             RCLCPP_WARN(this->get_logger(), "Cannot cancel trajectory, trajectory is currently being recorded.");
350 |             break;
351 |         }
352 |         case State::PLAYING:
353 |         {
354 |             RCLCPP_INFO(this->get_logger(), "Cancelling trajectory...");
355 |             currentState = State::IDLE;
356 |             followPathClient->async_cancel_all_goals();
357 |             RCLCPP_INFO(this->get_logger(), "Done.");
358 |             break;
359 |         }
360 |     }
361 | }
362 | 
363 | void WilnNode::saveLTRServiceCallback(const std::shared_ptr<wiln_msgs::srv::SaveMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::SaveMapTraj::Response> res)
364 | {
365 |     RCLCPP_INFO(this->get_logger(), "Saving LTR file %s", req->file_name.c_str());
366 | 
367 |     if (plannedTrajectory.poses.empty())
368 |     {
369 |         RCLCPP_WARN(this->get_logger(), "Cannot save empty trajectory.");
370 |         res->success = false;
371 |         return;
372 |     }
373 | 
374 |     if (req->file_name.empty())
375 |     {
376 |         RCLCPP_WARN(this->get_logger(), "File name cannot be empty.");
377 |         res->success = false;
378 |         return;
379 |     }
380 |     else if (std::ifstream(req->file_name))
381 |     {
382 |         RCLCPP_WARN(this->get_logger(), "File already exists.");
383 |         res->success = false;
384 |         return;
385 |     }
386 | 
387 |     res->success = saveLTR(req->file_name);
388 | 
389 |     if (!res->success)
390 |     {
391 |         RCLCPP_ERROR(this->get_logger(), "Failed to save LTR file.");
392 |     }
393 |     else
394 |     {
395 |         RCLCPP_INFO(this->get_logger(), "LTR file succesfully saved");
396 |     }
397 | }
398 | 
399 | bool WilnNode::saveLTR(std::string folderName)
400 | 
401 | {
402 |     // Create the folder if it doesn't exist
403 |     if (system(("mkdir -p " + folderName).c_str()) != 0)
404 |     {
405 |         RCLCPP_ERROR(this->get_logger(), "Failed to create folder: %s", folderName.c_str());
406 |         return false;
407 |     }
408 |     
409 |     saveMap(folderName);
410 | 
411 |     if (!std::ifstream(folderName + "/map.vtk"))  // Check that map was saved
412 |     {
413 |         RCLCPP_WARN(this->get_logger(), "Failed to save map or it is not yet done to be saved by the mapper.");
414 |         // return false;
415 |     }
416 | 
417 |     std::ofstream trajectoryFile(folderName + "/trajectory.txt", std::ios::app);
418 | 
419 |     // trajectoryFile << TRAJECTORY_DELIMITER << std::endl;
420 |     trajectoryFile << "# frame_id : " << plannedTrajectory.header.frame_id << std::endl;
421 | 
422 |     for (auto pose : plannedTrajectory.poses)
423 |     {
424 |         trajectoryFile << pose.pose.position.x << ","
425 |                        << pose.pose.position.y << ","
426 |                        << pose.pose.position.z << ","
427 |                        << pose.pose.orientation.x << ","
428 |                        << pose.pose.orientation.y << ","
429 |                        << pose.pose.orientation.z << ","
430 |                        << pose.pose.orientation.w << std::endl;
431 |     }
432 | 
433 |     trajectoryFile.close();
434 |     return true;
435 | }
436 | 
437 | void WilnNode::loadLTRServiceCallback(const std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Response> res)
438 | {
439 |     RCLCPP_INFO(this->get_logger(), "Loading LTR file %s", req->file_name.c_str());
440 | 
441 |     if (req->file_name.empty())
442 |     {
443 |         RCLCPP_WARN(this->get_logger(), "File name cannot be empty.");
444 |         res->success = false;
445 |         return;
446 |     }
447 |     else if (!std::ifstream(req->file_name))
448 |     {
449 |         RCLCPP_WARN(this->get_logger(), "File does not exist.");
450 |         res->success = false;
451 |         return;
452 |     }
453 | 
454 |     disableMapping();
455 |     res->success = loadLTR(req->file_name, false);
456 |     
457 |     if (!res->success)
458 |     {
459 |         RCLCPP_ERROR(this->get_logger(), "Failed to load LTR file.");
460 |     }
461 |     else
462 |     {
463 |         RCLCPP_INFO(this->get_logger(), "LTR file succesfully loaded");
464 |     }
465 | }
466 | 
467 | void WilnNode::loadLTRFromEndServiceCallback(const std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Request> req, std::shared_ptr<wiln_msgs::srv::LoadMapTraj::Response> res)
468 | {
469 |     RCLCPP_INFO(this->get_logger(), "Loading LTR file %s", req->file_name.c_str());
470 | 
471 |     if (req->file_name.empty())
472 |     {
473 |         RCLCPP_WARN(this->get_logger(), "File name cannot be empty.");
474 |         res->success = false;
475 |         return;
476 |     }
477 |     else if (!std::ifstream(req->file_name))
478 |     {
479 |         RCLCPP_WARN(this->get_logger(), "File does not exist.");
480 |         res->success = false;
481 |         return;
482 |     }
483 | 
484 | 	disableMapping();
485 |     res->success = loadLTR(req->file_name, true);
486 |     
487 |     if (!res->success)
488 |     {
489 |         RCLCPP_ERROR(this->get_logger(), "Failed to load LTR file.");
490 |     }
491 |     else
492 |     {
493 |         RCLCPP_INFO(this->get_logger(), "LTR file succesfully loaded");
494 |     }
495 | }
496 | 
497 | bool WilnNode::loadLTR(std::string fileName, bool fromEnd)
498 | {
499 |     struct stat statbuf;
500 |     if (stat(fileName.c_str(), &statbuf) == 0 && S_ISDIR(statbuf.st_mode)) {
501 |         std::string fileNameMap = fileName + "/map.vtk";
502 |         std::string fileNameTraj = fileName + "/trajectory.txt";
503 | 
504 |         std::ifstream mapFileVerification(fileNameMap);
505 |         if (!mapFileVerification.is_open()) {
506 |             RCLCPP_WARN(this->get_logger(), "LTR directory does not contain map.vtk. Skipping map loading and continuing with trajectory.");
507 |         }
508 |         
509 |         // Read trajectory
510 |         std::string line;
511 |         std::ifstream trajFile(fileNameTraj);
512 |         if (!trajFile.is_open()) {
513 |             RCLCPP_WARN(this->get_logger(), "LTR directory does not contain trajectory.txt.");
514 |             return false;
515 |         }
516 |         plannedTrajectory.poses.clear();
517 |         plannedTrajectory.header.stamp = this->now();
518 |         std::string pathFrameId;
519 | 
520 |         while (std::getline(trajFile, line)) {
521 |             if (line.empty() || line[0] == '#') {
522 |                 // Parse frame_id from comment line
523 |                 auto pos = line.find("frame_id : ");
524 |                 if (pos != std::string::npos) {
525 |                     pathFrameId = line.substr(pos + 11);
526 |                     plannedTrajectory.header.frame_id = pathFrameId;
527 |                 }
528 |                 continue;
529 |             }
530 |             std::stringstream ss(line);
531 |             std::string token;
532 |             std::vector<double> values;
533 |             geometry_msgs::msg::PoseStamped pose;
534 |             pose.header.frame_id = pathFrameId;
535 |             pose.header.stamp = this->now();
536 |             while (std::getline(ss, token, ',')) {
537 |                 values.push_back(std::stod(token));
538 |             }
539 |             if (values.size() == 7) {
540 |                 pose.header.stamp = this->now();
541 |                 pose.pose.position.x = values[0];
542 |                 pose.pose.position.y = values[1];
543 |                 pose.pose.position.z = values[2];
544 |                 pose.pose.orientation.x = values[3];
545 |                 pose.pose.orientation.y = values[4];
546 |                 pose.pose.orientation.z = values[5];
547 |                 pose.pose.orientation.w = values[6];
548 |                 plannedTrajectory.poses.push_back(pose);
549 |             }
550 |         }
551 |         trajFile.close();
552 | 
553 |         if (plannedTrajectory.poses.empty()) {
554 |             RCLCPP_WARN(this->get_logger(), "Trajectory file seems to contain no trajectory.");
555 |             return false;
556 |         }
557 | 
558 |         if (fromEnd) {
559 |             plannedTrajectory = reversePath(plannedTrajectory);
560 |         }
561 | 
562 |         loadMap(plannedTrajectory.poses.front().pose, fileNameMap);
563 |         // std::remove(TEMP_MAP_FILE.c_str());
564 |         publishPlannedTrajectory();
565 |         return true;
566 | 
567 |     }
568 |     else {
569 |         // Old method
570 |         std::ofstream mapFile("tmp/map.vtk");
571 |         std::ifstream ltrFile(fileName);
572 |         std::string line;
573 | 
574 |         // Parse Map
575 |         while (std::getline(ltrFile, line) && line.find(TRAJECTORY_DELIMITER) == std::string::npos)
576 |         {
577 |             mapFile << line << std::endl;
578 |         }
579 |         mapFile.close();
580 | 
581 |         if (!std::getline(ltrFile, line)) {
582 |             RCLCPP_WARN(this->get_logger(), "LTR file seems to contain no trajectory.");
583 |             return false;
584 |         }
585 | 
586 |         // Parse Trajectory
587 |         std::string pathFrameId = line.substr(FRAME_ID_START_POSITION);
588 | 
589 |         plannedTrajectory.poses.clear();
590 |         plannedTrajectory.header.frame_id = pathFrameId;
591 |         plannedTrajectory.header.stamp = this->now();
592 | 
593 |         geometry_msgs::msg::PoseStamped pose;
594 |         pose.header.frame_id = pathFrameId;
595 |         pose.header.stamp = this->now();
596 | 
597 |         while (std::getline(ltrFile, line))
598 |         {
599 |             if (line.find("changing direction") != std::string::npos)
600 |             {
601 |                 continue; // Ignore
602 |             } 
603 | 
604 |             std::stringstream ss(line);
605 |             std::string token;
606 |             std::vector<double> values;
607 |             while (std::getline(ss, token, ','))
608 |             {
609 |                 values.push_back(std::stod(token));
610 |             }
611 |             pose.pose.position.x = values[0];
612 |             pose.pose.position.y = values[1];
613 |             pose.pose.position.z = values[2];
614 |             pose.pose.orientation.x = values[3];
615 |             pose.pose.orientation.y = values[4];
616 |             pose.pose.orientation.z = values[5];
617 |             pose.pose.orientation.w = values[6];
618 |             plannedTrajectory.poses.push_back(pose);
619 |         }
620 |         ltrFile.close();
621 | 
622 |         if (plannedTrajectory.poses.empty())
623 |         {
624 |             RCLCPP_WARN(this->get_logger(), "LTR file seems to contain no trajectory.");
625 |             return false;
626 |         }
627 | 
628 |         if (fromEnd)
629 |         {
630 |             plannedTrajectory = reversePath(plannedTrajectory);
631 |         }
632 | 
633 |         loadMap(plannedTrajectory.poses.front().pose, "/tmp/map.vtk");
634 |         std::remove("/tmp/map.vtk");
635 |         publishPlannedTrajectory();
636 |         return true;
637 |     }
638 | }
639 | 
640 | void WilnNode::enableMapping()
641 | {
642 |     auto enableMappingRequest = std::make_shared<std_srvs::srv::Empty::Request>();
643 |     auto future = enableMappingClient->async_send_request(enableMappingRequest);
644 |     auto response = future.wait_for(5s);
645 | 
646 |     // TODO: implement feedback in mapper
647 |     // if (response->success)
648 | }
649 | 
650 | void WilnNode::disableMapping()
651 | {
652 |     auto disableMappingRequest = std::make_shared<std_srvs::srv::Empty::Request>();
653 | 	auto future = disableMappingClient->async_send_request(disableMappingRequest);
654 |     auto response = future.wait_for(5s);
655 | 
656 |     // TODO: implement feedback in mapper
657 | }
658 | 
659 | void WilnNode::saveMap(std::string folderName)
660 | {
661 |     auto saveMapRequest = std::make_shared<norlab_icp_mapper_ros::srv::SaveMap::Request>();
662 |     saveMapRequest->map_file_name.data = folderName + "/map.vtk";
663 | 	auto future = saveMapClient->async_send_request(saveMapRequest);
664 |     auto response = future.wait_for(1s);
665 | 
666 |     // TODO: implement feedback in mapper
667 | }
668 | 
669 | void WilnNode::loadMap(geometry_msgs::msg::Pose pose, std::string fileNameMap)
670 | {
671 |     auto loadMapRequest = std::make_shared<norlab_icp_mapper_ros::srv::LoadMap::Request>();
672 |     loadMapRequest->map_file_name.data = fileNameMap;
673 |     loadMapRequest->pose = pose;
674 |     auto future = loadMapClient->async_send_request(loadMapRequest);
675 |     auto response = future.wait_for(5s);
676 | 
677 |     // TODO: implement feedback in mapper
678 | }
679 | 
680 | void WilnNode::publishPlannedTrajectory()
681 | {
682 |     plannedTrajectoryPublisher->publish(plannedTrajectory);
683 | }
684 | 
685 | void WilnNode::publishRealTrajectory()
686 | {
687 |     realTrajectoryPublisher->publish(realTrajectory);
688 | }
689 | 
690 | void WilnNode::publishState()
691 | {
692 |     std_msgs::msg::UInt8 stateMsg;
693 |     stateMsg.data = static_cast<uint8_t>(currentState);
694 |     statePublisher->publish(stateMsg);
695 | }
696 | 
697 | void WilnNode::playLineServiceCallback(const std::shared_ptr<std_srvs::srv::Empty::Request> req, std::shared_ptr<std_srvs::srv::Empty::Response> res)
698 | {
699 |     if (plannedTrajectory.poses.empty())
700 |     {
701 |         RCLCPP_WARN(this->get_logger(), "Cannot play an empty trajectory.");
702 |         return;
703 |     }
704 | 
705 |     switch (currentState)
706 |     {
707 |         case State::IDLE:
708 |         {
709 |             RCLCPP_WARN(this->get_logger(), "Playing line.");
710 |             playLine();
711 |             currentState = State::PLAYING;
712 |             break;
713 |         }
714 |         case State::RECORDING:
715 |         {
716 |             RCLCPP_WARN(this->get_logger(), "Cannot play line while recording.");
717 |             break;
718 |         }
719 |         case State::PLAYING:
720 |         {
721 |             RCLCPP_WARN(this->get_logger(), "Trajectory is already being played.");
722 |             break;
723 |         }
724 |     }
725 | }
726 | 
727 | void WilnNode::playLine()
728 | {
729 |     robotPoseLock.lock();
730 |     nav_msgs::msg::Path lineTrajectory(plannedTrajectory);
731 | 
732 |     auto [lin_dist_start, ang_dist_start] = diffBetweenPoses(robotPose.pose, lineTrajectory.poses.front().pose);
733 |     auto [lin_dist_end, ang_dist_end] = diffBetweenPoses(robotPose.pose, lineTrajectory.poses.back().pose);
734 |     RCLCPP_INFO(this->get_logger(), " -> Distance from start: %f", lin_dist_start);
735 |     RCLCPP_INFO(this->get_logger(), " -> Distance from end: %f", lin_dist_end);
736 | 
737 |     // Reverse trajectory if robot is closer to end
738 |     if (lin_dist_start > lin_dist_end + 2.0)
739 |     {
740 |         RCLCPP_INFO(this->get_logger(), "Reversing trajectory.");
741 |         lineTrajectory = reversePath(lineTrajectory);
742 |     }
743 | 
744 |     // Flip trajectory if angle_diff is larger than M_PI/2
745 |     auto [lin_dist_final, ang_dist_final] = diffBetweenPoses(robotPose.pose, lineTrajectory.poses.front().pose);
746 |     RCLCPP_INFO(this->get_logger(), " -> Angle error: %f", ang_dist_final);
747 |     if (std::fabs(ang_dist_final) > M_PI/2)
748 |     {
749 |         RCLCPP_INFO(this->get_logger(), "Flipping trajectory.");
750 |         lineTrajectory = flipPath(lineTrajectory);
751 |     }
752 | 
753 |     robotPoseLock.unlock();
754 |     realTrajectory.poses.clear();
755 | 
756 |     // plannedTrajectory = lineTrajectory;
757 |     publishPlannedTrajectory();
758 |     sendFollowPathAction(lineTrajectory);
759 | }
760 | 
761 | void WilnNode::playLoopServiceCallback(const std::shared_ptr<wiln_msgs::srv::PlayLoop::Request> req, std::shared_ptr<wiln_msgs::srv::PlayLoop::Response> res)
762 | {
763 |     if (plannedTrajectory.poses.empty())
764 |     {
765 |         RCLCPP_WARN(this->get_logger(), "Cannot play an empty trajectory.");
766 |         return;
767 |     }
768 | 
769 |     switch (currentState)
770 |     {
771 |         case State::IDLE:
772 |         {
773 |             // Check if loop closure is within tolerances
774 |             auto [lin_dist, ang_dist] = diffBetweenPoses(plannedTrajectory.poses.front().pose, plannedTrajectory.poses.back().pose);
775 |             if (lin_dist > loopClosureLinearTolerance)
776 |             {
777 |                 RCLCPP_WARN(this->get_logger(), "Trajectory is not a loop, linear distance (%f) exceeds tolerance (%f).", lin_dist, loopClosureLinearTolerance);
778 |                 break;
779 |             }
780 |             if (std::fabs(ang_dist) > loopClosureAngularTolerance)
781 |             {
782 |                 RCLCPP_WARN(this->get_logger(), "Trajectory is not a loop, angular distance (%f) exceeds tolerance (%f).", std::fabs(ang_dist), loopClosureAngularTolerance);
783 |                 break;
784 |             }
785 | 
786 |             RCLCPP_WARN(this->get_logger(), "Playing loop %d times.", req->nb_loops);
787 |             playLoop(req->nb_loops);
788 |             currentState = State::PLAYING;
789 |             break;
790 |         }
791 |         case State::RECORDING:
792 |         {
793 |             RCLCPP_WARN(this->get_logger(), "Cannot play loop while recording.");
794 |             break;
795 |         }
796 |         case State::PLAYING:
797 |         {
798 |             RCLCPP_WARN(this->get_logger(), "Trajectory is already being played.");
799 |             break;
800 |         }
801 |     }
802 | }
803 | 
804 | void WilnNode::playLoop(int nbLoops)
805 | {
806 |     robotPoseLock.lock();
807 |     nav_msgs::msg::Path loopTrajectory;
808 |     loopTrajectory.header = plannedTrajectory.header;
809 | 
810 |     // Remove overlapping poses
811 |     auto cleanTrajectory = removePathOverlap(plannedTrajectory);
812 |     RCLCPP_INFO(this->get_logger(), "Removed %ld overlapping points at the start.", plannedTrajectory.poses.size() - cleanTrajectory.poses.size());
813 | 
814 |     // Repeat trajectory X times
815 |     for (int i = 0; i < nbLoops; ++i)
816 |     {
817 |         loopTrajectory.poses.insert(loopTrajectory.poses.end(), cleanTrajectory.poses.begin(), cleanTrajectory.poses.end());
818 |     }
819 | 
820 |     robotPoseLock.unlock();
821 |     realTrajectory.poses.clear();
822 | 
823 |     sendFollowPathAction(loopTrajectory);
824 |     // TODO: Check that a controller responded 
825 | }
826 | 
827 | void WilnNode::sendFollowPathAction(nav_msgs::msg::Path &path)
828 | {
829 |     auto disableMappingRequest = std::make_shared<std_srvs::srv::Empty::Request>();
830 |     disableMappingClient->async_send_request(disableMappingRequest);
831 | 
832 |     // TODO: validate action call
833 |     auto goal_msg = controller_msgs::action::FollowPath::Goal();
834 |     goal_msg.follower_options.init_mode.data = 1; // init_mode = 1 : continue
835 |     goal_msg.follower_options.velocity.data = trajectorySpeed;
836 |     goal_msg.path = path;
837 | 
838 |     auto send_goal_options = rclcpp_action::Client<controller_msgs::action::FollowPath>::SendGoalOptions();
839 |     send_goal_options.goal_response_callback = std::bind(&WilnNode::goalResponseCallback, this, std::placeholders::_1);
840 |     send_goal_options.feedback_callback = std::bind(&WilnNode::trajectoryFeedbackCallback, this, std::placeholders::_1, std::placeholders::_2);
841 |     send_goal_options.result_callback = std::bind(&WilnNode::trajectoryResultCallback, this, std::placeholders::_1);
842 |     followPathClient->async_send_goal(goal_msg, send_goal_options);
843 | }
844 | 
845 | void WilnNode::goalResponseCallback(const rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::SharedPtr &trajectoryGoalHandle)
846 | {
847 |     if (!trajectoryGoalHandle)
848 |     {
849 |         RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
850 |     }
851 |     else
852 |     {
853 |         RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
854 |     }
855 | }
856 | 
857 | void WilnNode::trajectoryFeedbackCallback(rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::SharedPtr,
858 |                                 const std::shared_ptr<const controller_msgs::action::FollowPath::Feedback> feedback)
859 | {
860 |     // TODO: program feedback callback
861 |     return;
862 | }
863 | 
864 | void WilnNode::trajectoryResultCallback(const rclcpp_action::ClientGoalHandle<controller_msgs::action::FollowPath>::WrappedResult &trajectory_result)
865 | {
866 |     if (trajectory_result.code == rclcpp_action::ResultCode::SUCCEEDED)
867 |     {
868 |         RCLCPP_INFO(this->get_logger(), "Successfully reached goal!");
869 |         RCLCPP_INFO(this->get_logger(), "--------------------------");
870 |     }
871 |     else
872 |     {
873 |         RCLCPP_WARN(this->get_logger(), "Trajectory goal was not reached...");
874 |     }
875 | 
876 |     currentState = State::IDLE;
877 | }
878 | 


--------------------------------------------------------------------------------