Skip to content

Commit

Permalink
Optimized the sim-to-real communication
Browse files Browse the repository at this point in the history
  • Loading branch information
@aalmrad
aalmrad committed Oct 9, 2024
1 parent 863cc2a commit 6bfb2be
Showing 1 changed file with 72 additions and 46 deletions.
118 changes: 72 additions & 46 deletions clearpath_manipulators/src/gazebo_to_real_robot_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,21 +16,33 @@ class GazeboToRealRobotBridge : public rclcpp::Node
{
public:
GazeboToRealRobotBridge()
: Node("gazebo_to_real_robot_bridge")
: Node("gazebo_to_real_robot_bridge"),
init_reached(false),
tol(0.02),
max_position_difference(0.0),
position_difference(0.0),
scaling_factor(3.0), // Initialize with default
total_time(0.0),
num_joints(7), // Initialize num_joints here
real_arm_joint_positions_(num_joints, 0.0), // Initialize vectors in initializer list
new_positions(num_joints, 0.0),
real_joint_positions_(num_joints, 0.0)
{
real_arm_joint_positions_.resize(7, 0.0);
// Subscribe to the Gazebo joint states
gazebo_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/a200_0000/platform/joint_states", 10,
std::bind(&GazeboToRealRobotBridge::gazebo_joint_states_callback, this, std::placeholders::_1));
// Set the QoS profile for the publisher to match the expected settings
rclcpp::QoS qos_profile = rclcpp::QoS(10)
.reliable() // Reliable QoS
.reliable() // Best effort QoS
.durability_volatile(); // Volatile durability
// Subscribe to the Gazebo joint states
gazebo_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/a200_0000/platform/joint_states",
qos_profile,
std::bind(&GazeboToRealRobotBridge::gazebo_joint_states_callback, this, std::placeholders::_1));


// Subscribe to the /joint_states topic
joint_state_sub_ = this->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", 10,
"/joint_states",
qos_profile,
std::bind(&GazeboToRealRobotBridge::joint_state_callback, this, std::placeholders::_1));

// Publisher for the physical robot's joint trajectory controller
Expand All @@ -55,6 +67,7 @@ class GazeboToRealRobotBridge : public rclcpp::Node
"joint_6",
"joint_7"
};

}

private:
Expand All @@ -63,45 +76,51 @@ class GazeboToRealRobotBridge : public rclcpp::Node
// Callback function to handle the incoming joint states from Gazebo
void gazebo_joint_states_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(joint_positions_mutex); // Lock the mutex to prevent race conditions
if(!init_reached){
rclcpp::sleep_for(std::chrono::milliseconds(1000));
}

trajectory_msgs::msg::JointTrajectory traj_msg;
traj_msg.joint_names = real_arm_joint_names_;

trajectory_msgs::msg::JointTrajectoryPoint traj_point;

// Filter only the arm joint positions from the incoming JointState message
for (const auto &sim_arm_joint_name : sim_arm_joint_names_)
// Resize the points vector to hold exactly one point
traj_msg.points.resize(1);
// Reference to the single point for easy access
trajectory_msgs::msg::JointTrajectoryPoint &traj_point = traj_msg.points[0];
traj_point.positions.resize(real_arm_joint_names_.size(), 0.0); // Pre-allocate space

// Extract arm joint positions from the incoming message without holding a lock
for (size_t i = 0; i < sim_arm_joint_names_.size(); ++i)
{
auto it = std::find(msg->name.begin(), msg->name.end(), sim_arm_joint_name);
auto it = std::find(msg->name.begin(), msg->name.end(), sim_arm_joint_names_[i]);
if (it != msg->name.end())
{
// Get the index of the arm joint in the message
auto index = std::distance(msg->name.begin(), it);

// Append the corresponding position to the trajectory point
traj_point.positions.push_back(msg->position[index]);
// Directly assign the corresponding position to the pre-sized vector
traj_point.positions[i] = msg->position[index];
}
}
// Copy real joint positions while holding the lock
{
std::lock_guard<std::mutex> lock(joint_positions_mutex);
real_joint_positions_ = real_arm_joint_positions_;
}

if(!init_reached){

std::cout << "Joints positional commands" << std::endl;
std::copy(traj_point.positions.begin(), traj_point.positions.end(), std::ostream_iterator<float>(std::cout, " "));
std::cout << "" << std::endl;
std::cout << "Real-life joints positons" << std::endl;
std::for_each(std::begin(real_arm_joint_positions_), std::end(real_arm_joint_positions_), [](double val) { std::cout << val << " "; });
std::cout << "" << std::endl;
//std::cout << "Joints positional commands" << std::endl;
//std::copy(traj_point.positions.begin(), traj_point.positions.end(), std::ostream_iterator<float>(std::cout, " "));
//std::cout << "" << std::endl;
//std::cout << "Real-life joints positons" << std::endl;
//std::for_each(std::begin(real_arm_joint_positions_), std::end(real_arm_joint_positions_), [](double val) { std::cout << val << " "; });
//std::cout << "" << std::endl;
// Calculate the time from start based on the difference between real and simulated joint positions
double max_position_difference = 0.0;

max_position_difference = 0.0;
for (size_t i = 0; i < traj_point.positions.size(); ++i)
{
// Calculate the absolute difference between real and simulated joint positions
double position_difference = std::abs(traj_point.positions[i] - real_arm_joint_positions_[i]);
position_difference = std::abs(traj_point.positions[i] - real_joint_positions_[i]);
if (position_difference > max_position_difference) {
max_position_difference = position_difference; // Track the maximum difference
}
Expand All @@ -115,10 +134,10 @@ class GazeboToRealRobotBridge : public rclcpp::Node
}

// Define a scaling factor for time (adjust this value as necessary)
double scaling_factor = 3.0; // This controls how much time is given based on the max difference
scaling_factor = 3.0; // This controls how much time is given based on the max difference

// Calculate time based on the difference
double total_time = max_position_difference * scaling_factor;
total_time = max_position_difference * scaling_factor;
// Set time_from_start based on the maximum position difference
traj_point.time_from_start.sec = static_cast<int>(total_time);
traj_point.time_from_start.nanosec = static_cast<int>((total_time - traj_point.time_from_start.sec) * 1e9);
Expand All @@ -127,60 +146,67 @@ class GazeboToRealRobotBridge : public rclcpp::Node
}
else{
traj_point.time_from_start.sec = 0;
traj_point.time_from_start.nanosec = 0;
}

// Add the trajectory point to the message
traj_msg.points.push_back(traj_point);

// Publish the trajectory message to the physical robot
robot_pub_->publish(traj_msg);
}

// Callback function to handle joint state messages coming from the rl robot
void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(joint_positions_mutex); // Lock the mutex to prevent race conditions

// Ensure that real_arm_joint_positions_ has the correct size (already resized)
real_arm_joint_positions_.clear();
real_arm_joint_positions_.resize(real_arm_joint_names_.size(), 0.0);


// Loop through the real_arm_joint_names_ and update the corresponding positions directly
// Extract joint positions without holding the lock
for (size_t i = 0; i < real_arm_joint_names_.size(); ++i)
{
auto it = std::find(msg->name.begin(), msg->name.end(), real_arm_joint_names_[i]);
if (it != msg->name.end())
{
// Get the index of the arm joint in the message
auto index = std::distance(msg->name.begin(), it);

// Assign the corresponding position directly to the vector
real_arm_joint_positions_[i] = msg->position[index];
new_positions[i] = msg->position[index];
}
}

// Now lock the mutex and update the shared vector
{
std::lock_guard<std::mutex> lock(joint_positions_mutex);
real_arm_joint_positions_ = new_positions;
}
}


rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr gazebo_sub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr robot_pub_;

size_t num_joints = 7;
// List of arm joint names you're interested in
std::vector<std::string> real_arm_joint_names_;
std::vector<std::string> sim_arm_joint_names_;
// Vector to store the real-life robot's joints positions
std::vector<double> real_arm_joint_positions_;
// Mutex to protect access to real_arm_joint_positions_
std::mutex joint_positions_mutex;
bool init_reached = false;
double tol = 0.04;
// Temporary variables to reduce the lock scope
std::vector<double> new_positions;
std::vector<double> real_joint_positions_;

bool init_reached;
double tol;
double max_position_difference;
double position_difference;
double scaling_factor;
double total_time;
};

int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<GazeboToRealRobotBridge>();
rclcpp::spin(node);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(node);
executor.spin(); // Use multi-threaded spinning
rclcpp::shutdown();
return 0;
}
}

0 comments on commit 6bfb2be

Please sign in to comment.