improve avoidance node to use ROS2

avoidance/include/avoidance/avoidance_node.h

@@ -40,10 +40,15 @@ class AvoidanceNode {
 
   rclcpp::executors::MultiThreadedExecutor cmdloop_executor_;
   rclcpp::executors::MultiThreadedExecutor statusloop_executor_;
-
   rclcpp::TimerBase::SharedPtr cmdloop_timer_;
   rclcpp::TimerBase::SharedPtr statusloop_timer_;
 
+  std::thread* statusloop_thread;
+  std::thread* cmdloop_thread;
+
+  rclcpp::Node::SharedPtr avoidance_node_status;
+  rclcpp::Node::SharedPtr avoidance_node_cmd;
+
   MAV_STATE companion_state_ = MAV_STATE::MAV_STATE_STANDBY;
 
   // PX4 Firmware parameters

avoidance/src/avoidance_node.cpp

@@ -29,29 +29,26 @@ AvoidanceNode::~AvoidanceNode() {}
 void AvoidanceNode::init() {
   setSystemStatus(MAV_STATE::MAV_STATE_BOOT);
 
-  // Command loop executor
-  auto cmd_loop_callback = [&]() { /* Empty ?? */ };
-  auto avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd");
-  cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, cmd_loop_callback);
+  avoidance_node_cmd = rclcpp::Node::make_shared("avoidance_node_cmd");
+  cmdloop_timer_ = avoidance_node_cmd->create_wall_timer(cmdloop_dt_, [&](){});
   cmdloop_executor_.add_node(avoidance_node_cmd);
 
-  // Status loop executor
-  auto status_loop_callback = [&]() { publishSystemStatus(); };
-  auto avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status");
+  // Start cmdloop thread
+  cmdloop_thread = new std::thread([&]() {
+    cmdloop_executor_.spin();
+  });
+
+  avoidance_node_status = rclcpp::Node::make_shared("avoidance_node_status");
   // This is a passthrough that replaces the usage of Mavlink Heartbeats
   telemetry_status_pub_ =
         avoidance_node_status->create_publisher<px4_msgs::msg::TelemetryStatus>("TelemetryStatus_PubSubTopic", 1);
-  statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, status_loop_callback);
+  statusloop_timer_ = avoidance_node_status->create_wall_timer(statusloop_dt_, [&](){ publishSystemStatus(); });
   statusloop_executor_.add_node(avoidance_node_status);
 
-  auto statusloop_spin_executor = [&]() {
+  // Start statusloop thread
+  statusloop_thread = new std::thread([&]() {
     statusloop_executor_.spin();
-  };
-
-  // Launch both executors
-  std::thread execution_thread(statusloop_spin_executor);
-  cmdloop_executor_.spin();
-  execution_thread.join();
+  });
 }
 
 void AvoidanceNode::setSystemStatus(MAV_STATE state) {

avoidance/src/common.cpp

@@ -304,7 +304,7 @@ void transformToTrajectory(px4_msgs::msg::VehicleTrajectoryWaypoint& obst_avoid,
   fillUnusedTrajectoryPoint(obst_avoid.waypoints[3]);
   fillUnusedTrajectoryPoint(obst_avoid.waypoints[4]);
 
-  for (size_t i = 0; i < sizeof(obst_avoid.waypoints); i++) {
+  for (size_t i = 0; i < obst_avoid.waypoints.size(); i++) {
     obst_avoid.waypoints[i].timestamp = obst_avoid.timestamp;
     obst_avoid.waypoints[i].point_valid = false;
   }

avoidance/src/transform_buffer.cpp

@@ -26,9 +26,10 @@ bool TransformBuffer::interpolateTransform(const geometry_msgs::msg::TransformSt
   tf2::Vector3 tf_earlier_translation;
   tf2::Quaternion tf_earlier_rotation;
   tf2::Quaternion tf_later_rotation;
-  tf2::fromMsg(tf_earlier_translation, tf_earlier.transform.translation);
-  tf2::fromMsg(tf_earlier_rotation, tf_earlier.transform.rotation);
-  tf2::fromMsg(tf_later_rotation, tf_later.transform.rotation);
+  tf_earlier_translation.setValue(tf_earlier.transform.translation.x, tf_earlier.transform.translation.y, tf_earlier.transform.translation.z);
+  // tf2::fromMsg(tf_earlier.transform.translation, tf_earlier_translation);
+  tf2::fromMsg(tf_earlier.transform.rotation, tf_earlier_rotation);
+  tf2::fromMsg(tf_later.transform.rotation, tf_later_rotation);
 
   const tf2::Vector3 translation = tf_earlier_translation * (1.f - tau) + tf_earlier_translation * tau;
   const tf2::Quaternion rotation = tf_earlier_rotation.slerp(tf_later_rotation, tau);