diff --git a/src/dcaiti_control/launch/rsp.launch.py b/src/dcaiti_control/launch/rsp.launch.py index ffee96c..cbff180 100644 --- a/src/dcaiti_control/launch/rsp.launch.py +++ b/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 diff --git a/src/dcaitirobot/dcaitirobot/tf2pleasework.py b/src/dcaitirobot/dcaitirobot/tf2pleasework.py new file mode 100644 index 0000000..979e2bc --- /dev/null +++ b/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() diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h b/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h index e3d22cc..1b8ab1c 100644 --- a/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h +++ b/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; diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h b/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h index 6416a75..f0180d0 100644 --- a/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h +++ b/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: diff --git a/src/diffdrive_arduino/src/arduino_comms.cpp b/src/diffdrive_arduino/src/arduino_comms.cpp index 5d03e04..a1353df 100644 --- a/src/diffdrive_arduino/src/arduino_comms.cpp +++ b/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() diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp index ea8192c..a4fb54b 100644 --- a/src/diffdrive_arduino/src/diffdrive_arduino.cpp +++ b/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()) { diff --git a/src/diffdrive_arduino/src/diffdrive_robot.cpp b/src/diffdrive_arduino/src/diffdrive_robot.cpp index 34dd02f..2c3fabf 100644 --- a/src/diffdrive_arduino/src/diffdrive_robot.cpp +++ b/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(); diff --git a/src/diffdrive_arduino/src/fake_robot.cpp b/src/diffdrive_arduino/src/fake_robot.cpp index 6794043..cbc89e3 100644 --- a/src/diffdrive_arduino/src/fake_robot.cpp +++ b/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 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