Hi everyone,
I’m working on a ROS2 project where I want one robot (“Rick”) to chase another robot (“Morty”) using the tf2_ros transform between their base_link frames. I’ve written a node that subscribes to the transform and calculates the required linear and angular velocities for Rick to follow Morty. Moving Morty with teleops often enough results in Morty not being able to be controlled. I’m facing a persistent issue where Rick doesn’t seem to follow Morty at all, and instead, he moves erratically.
Could there be an underlying issue with how I’m processing the transform data or the way the control logic is applied?
Has anyone encountered a similar issue or have any insights into what might be going wrong?
robot_chase.cpp:
#include <chrono>
#include <cmath>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <thread>
class RobotChase : public rclcpp::Node {
public:
RobotChase()
: Node("robot_chase_node"), tf_buffer_(this->get_clock()),
tf_listener_(tf_buffer_) {
// Publisher for Rick's velocity commands on the /rick/cmd_vel topic
velocity_publisher_ =
this->create_publisher<geometry_msgs::msg::Twist>("/rick/cmd_vel", 10);
// Introducing a small delay to ensure the TF listener has time to start
// receiving data
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// Timer to periodically call the update function every 100 milliseconds
timer_ = this->create_wall_timer(std::chrono::milliseconds(100),
std::bind(&RobotChase::update, this));
// Control gains as specified
kp_yaw_ = 1.5;
kp_distance_ = 0.5;
// Initial debug statement
RCLCPP_INFO(this->get_logger(), "RobotChase node initialized.");
}
private:
void update() {
geometry_msgs::msg::TransformStamped transform_stamped;
try {
// Attempt to get the latest transform from Morty's base_link to Rick's
// base_link
transform_stamped = tf_buffer_.lookupTransform(
"rick/base_link", "morty/base_link", tf2::TimePointZero);
RCLCPP_INFO(this->get_logger(), "Transform received: x: %f, y: %f, z: %f",
transform_stamped.transform.translation.x,
transform_stamped.transform.translation.y,
transform_stamped.transform.translation.z);
} catch (tf2::TransformException &ex) {
// If the transform is not available, log a warning and exit the function
RCLCPP_WARN(this->get_logger(), "Could not transform: %s", ex.what());
return;
}
// Relative position of Morty to Rick
double dx = transform_stamped.transform.translation.x;
double dy = transform_stamped.transform.translation.y;
RCLCPP_INFO(this->get_logger(), "Relative position: dx: %f, dy: %f", dx,
dy);
// Calculate the distance error between Rick and Morty
double error_distance = sqrt(dx * dx + dy * dy);
RCLCPP_INFO(this->get_logger(), "Error Distance: %f", error_distance);
// Calculate the angular error (yaw) based on the relative position of Morty
// to Rick
double error_yaw = atan2(dy, dx);
RCLCPP_INFO(this->get_logger(), "Error Yaw (radians): %f", error_yaw);
RCLCPP_INFO(this->get_logger(), "Error Yaw (degrees): %f",
error_yaw * 180.0 / M_PI);
// Calculate the linear and angular velocity commands using proportional
// control
double linear_velocity = kp_distance_ * error_distance;
double angular_velocity = kp_yaw_ * error_yaw;
RCLCPP_INFO(this->get_logger(), "Calculated linear velocity: %f",
linear_velocity);
RCLCPP_INFO(this->get_logger(), "Calculated angular velocity: %f",
angular_velocity);
// Create a Twist message to send the velocity commands to Rick
auto twist_msg = geometry_msgs::msg::Twist();
twist_msg.linear.x = linear_velocity; // Set linear velocity
twist_msg.angular.z = angular_velocity; // Set angular velocity
// Publish the Twist message to the /rick/cmd_vel topic
velocity_publisher_->publish(twist_msg);
RCLCPP_INFO(this->get_logger(),
"Twist message published: linear.x = %f, angular.z = %f",
twist_msg.linear.x, twist_msg.angular.z);
}
// Member variables
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr velocity_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
// Control gains as specified
double kp_yaw_;
double kp_distance_;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv); // Initialize the ROS2 client library
auto node = std::make_shared<RobotChase>(); // Create an instance of the
// RobotChase node
rclcpp::spin(node); // Keep the node running, processing callbacks and timers
rclcpp::shutdown(); // Shutdown the ROS2 client library when done
return 0;
}
dual_odom_to_tf_broadcaster.py:
#! /usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, DurabilityPolicy, QoSProfile
import tf2_ros
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import LaserScan
class DualOdomToTF(Node):
def __init__(self):
super().__init__('dual_odom_to_tf_broadcaster_node')
self.robot_names = ["rick", "morty"]
self.transforms = {}
self.broadcasters = {}
# Using reliable QoS for critical data
qos_reliable = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.VOLATILE)
for robot_name in self.robot_names:
# Transform from world to odom
transform_world_to_odom = TransformStamped()
transform_world_to_odom.header.stamp = self.get_clock().now().to_msg()
transform_world_to_odom.header.frame_id = "world"
transform_world_to_odom.child_frame_id = f"{robot_name}/odom"
transform_world_to_odom.transform.translation.x = 0.0
transform_world_to_odom.transform.translation.y = 0.0
transform_world_to_odom.transform.translation.z = 0.0
transform_world_to_odom.transform.rotation.x = 0.0
transform_world_to_odom.transform.rotation.y = 0.0
transform_world_to_odom.transform.rotation.z = 0.0
transform_world_to_odom.transform.rotation.w = 1.0
# Broadcasting the static transform
self.static_broadcaster.sendTransform(transform_world_to_odom)
# Transform from odom to base_link
transform_base = TransformStamped()
transform_base.header.frame_id = f"{robot_name}/odom"
transform_base.child_frame_id = f"{robot_name}/base_link"
self.transforms[f"{robot_name}/base_link"] = transform_base
# Single broadcaster for all transforms
self.broadcasters[robot_name] = tf2_ros.TransformBroadcaster(self)
# Subscriber for each robot's odometry
self.create_subscription(
Odometry,
f"{robot_name}/odom",
self.create_odom_callback(robot_name),
qos_reliable
)
# Timer set to call broadcast_transforms at a 0.1-second interval
self.timer = self.create_timer(0.05, self.broadcast_transforms)
self.get_logger().info("Dual robot odom to TF broadcaster node is ready!")
def create_odom_callback(self, robot_name):
def callback(odom_msg):
self.update_transforms(odom_msg, robot_name)
return callback
def broadcast_transforms(self):
for robot_name in self.robot_names:
broadcaster = self.broadcasters[robot_name]
broadcaster.sendTransform(self.transforms[f"{robot_name}/base_link"])
# def odom_callback(self, odom_msg, robot_name):
# self.update_transforms(odom_msg, robot_name)
def update_transforms(self, odom_msg, robot_name):
# Update base_link transform
base_transform = self.transforms[f"{robot_name}/base_link"]
base_transform.header.stamp = odom_msg.header.stamp
base_transform.transform.translation.x = odom_msg.pose.pose.position.x
base_transform.transform.translation.y = odom_msg.pose.pose.position.y
base_transform.transform.translation.z = odom_msg.pose.pose.position.z
base_transform.transform.rotation = odom_msg.pose.pose.orientation
# Log transform updates for debugging
self.get_logger().debug(f'Updated transform for {robot_name} at time {odom_msg.header.stamp.sec}.{odom_msg.header.stamp.nanosec}')
def main(args=None):
rclpy.init(args=args)
node = DualOdomToTF()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Thanks in advance,