ASD Onboarding Finished

.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "functional": "cpp",
+        "memory": "cpp"
+    }
+}

docker/robot/control.Dockerfile

@@ -4,6 +4,10 @@ FROM ros:humble AS base
 # ADD DEPENDENCIES HERE
 
 ENV DEBIAN_FRONTEND noninteractive
+
+RUN apt-get update && apt-get install -y \
+    ros-$ROS_DISTRO-nav-msgs  # Install the nav_msgs package
+
 RUN sudo chsh -s /bin/bash
 ENV SHELL=/bin/bash
 

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,9 @@ 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,34 @@
 #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();
+    void timer_callback();
+    void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+    void goal_point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg);
 
   private:
     robot::ControlCore control_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_subscriber_;
+    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr goal_point_subscriber_;
+
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+    double Kp_linear;
+    double Kp_angular;
+    bool goal_point_received;
+
+    geometry_msgs::msg::PointStamped transformed_goal_point_;
+    geometry_msgs::msg::PointStamped original_goal_point_;
+
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr control_publisher_;
 };
 
 #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

@@ -1,10 +1,89 @@
 #include <memory>
-
 #include "control_node.hpp"
 
-ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore())
-{
-
+
+ControlNode::ControlNode(): Node("control"), control_(robot::ControlCore()){
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&ControlNode::timer_callback, this));
+
+  odometry_subscriber_ = this->create_subscription<nav_msgs::msg::Odometry>
+  ("/model/robot/odometry", 20, std::bind(&ControlNode::odometry_callback, this, std::placeholders::_1));
+
+  goal_point_subscriber_ = this->create_subscription<geometry_msgs::msg::PointStamped>
+  ("/goal_point", 20, std::bind(&ControlNode::goal_point_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_);
+
+  control_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 20);
+
+  Kp_linear = 0.3;
+  Kp_angular = 0.3;
+
+  original_goal_point_.point.x = 0.0;
+  original_goal_point_.point.y = 0.0;
+  original_goal_point_.point.z = 0.0;
+
+  transformed_goal_point_.point.x = 0.0;
+  transformed_goal_point_.point.y = 0.0;
+  transformed_goal_point_.point.z = 0.0;
+}
+
+void ControlNode::timer_callback(){
+  RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Entered Timer Callback");
+
+    // Transform goal point from sim_world to robot world
+  geometry_msgs::msg::TransformStamped transform;
+
+  if (goal_point_received){
+    try {
+        // Get the transform from "sim_world" to "robot"
+        transform = tf_buffer_->lookupTransform("robot", "sim_world", tf2::TimePointZero);
+
+        // Transform the goal_point to the robot's frame
+        tf2::doTransform(original_goal_point_, transformed_goal_point_, transform);
+
+        // Print the transformed coordinates
+        RCLCPP_INFO(this->get_logger(), "Transformed goal_point - x: %f, y: %f, z: %f",
+                    transformed_goal_point_.point.x,
+                    transformed_goal_point_.point.y,
+                    transformed_goal_point_.point.z);
+    } catch (const tf2::TransformException &ex) {
+        RCLCPP_WARN(this->get_logger(), "Could not transform goal_point: %s", ex.what());
+    }
+  }
+
+  double linear_control = Kp_linear * transformed_goal_point_.point.x;
+  double angular_control = Kp_angular * transformed_goal_point_.point.y; 
+
+  auto control_message = geometry_msgs::msg::Twist();
+  control_message.linear.x = linear_control;
+  control_message.angular.z = angular_control;
+  control_publisher_->publish(control_message); 
+
+}
+
+void ControlNode::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+  // Extract x, y, z coordinates from the odometry message
+  double x = msg->pose.pose.position.x;
+  double y = msg->pose.pose.position.y;
+  double z = msg->pose.pose.position.z;
+
+  // Print the coordinates to the console
+  RCLCPP_INFO(this->get_logger(), "Odometry coordinates - x: %f, y: %f, z: %f", x, y, z);
+}
+
+void ControlNode::goal_point_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg) {
+  // Print Goal Point
+  double x = msg->point.x;
+  double y = msg->point.y;
+  double z = msg->point.z;
+  RCLCPP_INFO(this->get_logger(), "Received PointStamped coordinates - x: %f, y: %f, z: %f", x, y, z);
+
+  original_goal_point_.point.x = msg->point.x;
+  original_goal_point_.point.y = msg->point.y;
+  original_goal_point_.point.z = msg->point.z;
+
+  goal_point_received = true;
 }
 
 int main(int argc, char ** argv)
@@ -14,3 +93,7 @@ int main(int argc, char ** argv)
   rclcpp::shutdown();
   return 0;
 }
+
+//rclcpp::get_logger("rclcpp")
+//this->get_logger()
+

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 = 15;
 
 public:
   /**

watod

@@ -37,7 +37,7 @@ function run_compose {
         PROFILES="$(source $PROFILES_DIR/.env && printf -- "-f profiles/docker-compose.%s.yaml " ${ACTIVE_PROFILES[@]})"
     fi
 
-    DOCKER_BUILDKIT=${DOCKER_BUILDKIT:-1} docker-compose ${PROFILES[@]} "$@"
+    DOCKER_BUILDKIT=${DOCKER_BUILDKIT:-1} docker compose ${PROFILES[@]} "$@"
 }
 
 # in case you need help

watod-config.sh

@@ -8,15 +8,15 @@
 ##   - robot            :   starts up robot nodes
 ##   - samples          :   starts up sample nodes for reference
 
-# ACTIVE_PROFILES=""
+ACTIVE_PROFILES="robot vis_tools gazebo"
 
 
 ## Name to append to docker containers. DEFAULT = <your_watcloud_username>
 
-# COMPOSE_PROJECT_NAME=""
+COMPOSE_PROJECT_NAME="rodneyshdong"
 
 
 ## Tag to use. Images are formatted as <IMAGE_NAME>:<TAG> with forward slashes replaced with dashes.
 ## DEFAULT = <your_current_github_branch> 
 
-# TAG=""
+TAG="rodneydong_training"