Draft: Alyzen Onboarding

src/robot/control/CMakeLists.txt

@@ -19,6 +19,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
 
 # Compiles source files into a library
 # A library is not executed, instead other executables can link
@@ -40,7 +41,8 @@ ament_target_dependencies(control_lib
   tf2
   tf2_ros
   tf2_geometry_msgs
-  std_msgs)
+  std_msgs
+  nav_msgs)
 
 # Create ROS2 node executable from source files
 add_executable(control_node src/control_node.cpp)

src/robot/control/include/control_node.hpp

@@ -10,15 +10,31 @@
 #include "tf2/exceptions.h"
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/buffer.h"
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-#include "std_msgs/msg/string.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 
 class ControlNode : public rclcpp::Node {
   public:
     ControlNode();
 
   private:
     robot::ControlCore control_;
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr subscriber_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    void timer_callback();
+    void sub_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg);
+
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    geometry_msgs::msg::PointStamped goal_point_;
+
+    double kp_linear_;
+    double kp_angular_;
+
+    bool goal_recieved_;
 };
 
 #endif

src/robot/control/package.xml

@@ -16,6 +16,7 @@
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>nav_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/robot/control/src/control_node.cpp

@@ -4,7 +4,55 @@
 
 ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore())
 {
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ControlNode::timer_callback, this));
+  publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 20);
+  subscriber_= this->create_subscription<geometry_msgs::msg::PointStamped>("/goal_point", 20, std::bind(&ControlNode::sub_callback, this, std::placeholders::_1));
 
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+  kp_linear_ = 0.5;
+  kp_angular_ = 0.5;
+}
+
+void ControlNode::timer_callback() {
+
+  RCLCPP_INFO(this->get_logger(), "Timer Callback");
+  if (goal_recieved_ ==  true) {
+
+
+    geometry_msgs::msg::TransformStamped transform;
+    auto transformed_point = geometry_msgs::msg::PointStamped();
+
+
+    try {
+        transform = tf_buffer_->lookupTransform("robot", "sim_world", tf2::TimePointZero);
+        tf2::doTransform(goal_point_, transformed_point, transform);
+    } catch (const tf2::TransformException & ex) {
+        RCLCPP_INFO(this->get_logger(), "Could not transform %s", ex.what());
+    }
+
+    double error_x = kp_linear_ * transformed_point.point.x;
+    double error_y = kp_angular_ * transformed_point.point.y;
+
+    auto message = geometry_msgs::msg::Twist();
+    message.linear.x = error_x;
+    message.angular.z = error_y;
+
+    publisher_->publish(message);
+
+  }
+}
+
+void ControlNode::sub_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) {
+  auto x = msg->point.x;
+  auto y = msg->point.y;
+  auto z = msg->point.z;
+  RCLCPP_INFO(this->get_logger(), "Received PointStamped: x=%.2f, y=%.2f, z=%.2f", x, y, z);
+
+  goal_recieved_ = true;
+
+  goal_point_ = *msg;
 }
 
 int main(int argc, char ** argv)

src/samples/cpp/transformer/include/transformer_core.hpp

@@ -17,7 +17,7 @@ class TransformerCore
 {
 public:
   // Size of buffer before processed messages are published.
-  static constexpr int BUFFER_CAPACITY = 10;
+  static constexpr int BUFFER_CAPACITY = 1;
 
 public:
   /**

watod-config.sh

@@ -8,7 +8,7 @@
 ##   - robot            :   starts up robot nodes
 ##   - samples          :   starts up sample nodes for reference
 
-# ACTIVE_PROFILES=""
+ACTIVE_PROFILES="vis_tools gazebo robot"
 
 
 ## Name to append to docker containers. DEFAULT = <your_watcloud_username>