galactic adjustments

src/dcaiti_control/launch/rsp.launch.py

@@ -20,6 +20,7 @@ def generate_launch_description():
     # Process the URDF file
     pkg_path = os.path.join(get_package_share_directory('dcaiti_control'))
     xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
+    # xacro_file = "/home/nvidia/ros2_humble/src/turtlebot3_simulations/turtlebot3_gazebo/urdf/turtlebot3_waffle.urdf"
     robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' use_sim:=', use_sim_time])
     
     # Create a robot_state_publisher node

src/dcaitirobot/dcaitirobot/tf2pleasework.py

@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.node import Node
+import tf2_ros
+import geometry_msgs.msg as gm
+
+
+class TF2PublisherNode(Node):
+
+    def __init__(self):
+        super().__init__('tf2_publisher_node')
+        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
+        
+        self.timer = self.create_timer(0.1, self.publish_identity_transform)
+
+        self.x = 0.0
+
+    def publish_identity_transform(self):
+        transform_stamped = gm.TransformStamped()
+        transform_stamped.header.stamp = self.get_clock().now().to_msg()
+        transform_stamped.header.frame_id = 'odom'
+        transform_stamped.child_frame_id = 'base_link'
+
+        # Identity transform (no translation, no rotation)
+        transform_stamped.transform.translation.x = self.x
+        transform_stamped.transform.translation.y = 0.0
+        transform_stamped.transform.translation.z = 0.0
+        transform_stamped.transform.rotation.x = 0.0
+        transform_stamped.transform.rotation.y = 0.0
+        transform_stamped.transform.rotation.z = 0.0
+        transform_stamped.transform.rotation.w = 1.0
+
+
+        self.x += 0.01
+
+        self.tf_broadcaster.sendTransform(transform_stamped)
+
+        transform_stamped = gm.TransformStamped()
+        transform_stamped.header.stamp = self.get_clock().now().to_msg()
+        transform_stamped.header.frame_id = 'base_link'
+        transform_stamped.child_frame_id = 'velodyne'
+
+        # Identity transform (no translation, no rotation)
+        transform_stamped.transform.translation.x = 0.0
+        transform_stamped.transform.translation.y = self.x
+        transform_stamped.transform.translation.z = 0.0
+        transform_stamped.transform.rotation.x = 0.0
+        transform_stamped.transform.rotation.y = 0.0
+        transform_stamped.transform.rotation.z = 0.0
+        transform_stamped.transform.rotation.w = 1.0
+
+        self.tf_broadcaster.sendTransform(transform_stamped)
+
+      
+
+
+        self.get_logger().info('Published identity transform')
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    tf2_publisher = TF2PublisherNode()
+    rclpy.spin(tf2_publisher)
+
+    tf2_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h

@@ -14,7 +14,7 @@
 #include "wheel.h"
 #include "arduino_comms.h"
 
-using hardware_interface::CallbackReturn;
+// using hardware_interface::CallbackReturn;
 using hardware_interface::return_type;
 
 namespace diffdrive_arduino
@@ -37,9 +37,11 @@ public:
 
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
-  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  // return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  return_type read() override;
 
-  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  // return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  return_type write() override;
 
 
 

src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h

@@ -13,7 +13,7 @@
 #include "config.h"
 #include "wheel.h"
 
-using hardware_interface::CallbackReturn;
+//using CallbackReturn;
 using hardware_interface::return_type;
 
 class FakeRobot : public hardware_interface::SystemInterface
@@ -33,9 +33,11 @@ public:
 
   CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
-  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+//  return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  return_type read() override;
 
-  return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  // return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
+  return_type write() override;
 
 
 private:

src/diffdrive_arduino/src/arduino_comms.cpp

@@ -14,7 +14,6 @@ void ArduinoComms::setup(const std::string &serial_device, int32_t baud_rate, in
     serial::Timeout tt = serial::Timeout::simpleTimeout(timeout_ms);
     serial_conn_.setTimeout(tt); // This should be inline except setTimeout takes a reference and so needs a variable
     serial_conn_.open();
-
 }
 
 void ArduinoComms::sendEmptyMsg()

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -90,7 +90,7 @@ CallbackReturn DiffDriveArduino::on_deactivate(const rclcpp_lifecycle::State & /
   return CallbackReturn::SUCCESS;
 }
 
-return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+return_type DiffDriveArduino::read()
 {
 
   // TODO fix chrono duration
@@ -114,7 +114,7 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
   return return_type::OK;  
 }
 
-return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+return_type DiffDriveArduino::write()
 {
   if (!arduino_.connected())
   {

src/diffdrive_arduino/src/diffdrive_robot.cpp

@@ -4,6 +4,7 @@
 
 int main(int argc, char **argv)
 {
+  RCLCPP_INFO(logger_, "STARTING FILE");
   ros::init(argc, argv, "diffdrive_robot");
   ros::NodeHandle n("~");
 
@@ -17,9 +18,11 @@ int main(int argc, char **argv)
   n.getParam("enc_counts_per_rev", robot_cfg.enc_counts_per_rev);
   n.getParam("robot_loop_rate", robot_cfg.loop_rate);
   
-
+  RCLCPP_INFO(logger_, "STARTING DIFFDRIVE");
   DiffDriveArduino robot(robot_cfg);
+  RCLCPP_INFO(logger_, "FINISHED DIFFDRIVE");
   controller_manager::ControllerManager cm(&robot);
+  RCLCPP_INFO(logger_, "FINISHED CONTROLLER");
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
@@ -30,6 +33,7 @@ int main(int argc, char **argv)
 
   while (ros::ok())
   {
+    RCLCPP_INFO(logger_, "Entered robot read/write loop.");
     robot.read();
     cm.update(robot.get_time(), robot.get_period());
     robot.write();

src/diffdrive_arduino/src/fake_robot.cpp

@@ -14,7 +14,7 @@ CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info)
   return CallbackReturn::ERROR;
 }
 
-  RCLCPP_INFO(logger_, "Configuring...");
+  RCLCPP_INFO(logger_, "Configuring Fakerobot...");
 
   time_ = std::chrono::system_clock::now();
 
@@ -29,7 +29,7 @@ CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info)
   l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev);
   r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev);
 
-  RCLCPP_INFO(logger_, "Finished Configuration");
+  RCLCPP_INFO(logger_, "Finished Configuration Fakerobot");
 
   return CallbackReturn::SUCCESS;
 }
@@ -62,7 +62,8 @@ std::vector<hardware_interface::CommandInterface> FakeRobot::export_command_inte
 
 
 
-hardware_interface::return_type FakeRobot::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+// hardware_interface::return_type FakeRobot::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+hardware_interface::return_type FakeRobot::read()
 {
 
   // TODO fix chrono duration
@@ -83,7 +84,8 @@ hardware_interface::return_type FakeRobot::read(const rclcpp::Time & /* time */,
   
 }
 
-hardware_interface::return_type FakeRobot::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+// hardware_interface::return_type FakeRobot::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
+hardware_interface::return_type FakeRobot::write()
 {
 
   // Set the wheel velocities to directly match what is commanded