Simulation Director, runs multiple sims

Author: tomputer-g

src/FVD_Dual_HRI.cpp

@@ -9,6 +9,8 @@
 #include "vader_msgs/GripperCommand.h"
 #include "vader_msgs/CutterCommand.h"
 
+#include "vader_msgs/HarvestResult.h"
+
 #include "vader_msgs/PlanningRequest.h"
 
 #include <ros/ros.h>
@@ -58,6 +60,8 @@ class VADERStateMachine
     // End effector talking to dynamixel node
     ros::Publisher gripperCommandPub, cutterCommandPub;
 
+    ros::Publisher resultStatusPub; //publishes to /harvest_result
+
     /*
     Input: cameraFrameMsg containing the pose of the fruit in the camera frame from the coarse/fine pepper estimate
     Output: result containing the pose of the fruit in the world frame using TF2.
@@ -96,10 +100,10 @@ class VADERStateMachine
                 peduncle_pose,
                 "world",
                 ros::Duration(3.0));
-            ROS_INFO("Transformed pose: x=%f, y=%f, z=%f",
-                     transformed_pose.pose.position.x,
-                     transformed_pose.pose.position.y,
-             transformed_pose.pose.position.z);
+            // ROS_INFO("Transformed pose: x=%f, y=%f, z=%f",
+            //          transformed_pose.pose.position.x,
+            //          transformed_pose.pose.position.y,
+            //  transformed_pose.pose.position.z);
             result->fruit_data.pose.position.x = transformed_pose.pose.position.x;
             result->fruit_data.pose.position.y = transformed_pose.pose.position.y;
             result->fruit_data.pose.position.z = transformed_pose.pose.position.z;
@@ -149,6 +153,17 @@ class VADERStateMachine
         ROS_INFO_STREAM("[" << currentState << "]: " << message);
     }
 
+    void _publishResultStatus(int result_code, const std::string &reason)
+    {
+        vader_msgs::HarvestResult resultMsg;
+        resultMsg.result = result_code;
+        resultMsg.reason = reason;
+        resultStatusPub.publish(resultMsg);
+        ros::Duration(1.0).sleep();
+        resultStatusPub.publish(resultMsg);
+        ROS_WARN_STREAM("Published harvest result: " << result_code << ", reason: " << reason);
+    }
+
 public:
     VADERStateMachine() : currentState(State::HomeGripper)
     {
@@ -161,6 +176,8 @@ class VADERStateMachine
 
         gripperCommandPub = n.advertise<vader_msgs::GripperCommand>("/gripper_command", 10);
         cutterCommandPub = n.advertise<vader_msgs::CutterCommand>("/cutter_command", 10);
+
+        resultStatusPub = n.advertise<vader_msgs::HarvestResult>("/harvest_status", 10);
     }
 
     void setCoarsePoseEstimate(const vader_msgs::Pepper::ConstPtr &msg)
@@ -209,6 +226,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to home gripper.");
                         _logWithState("Failed to call planning service for gripper homing.");
                         currentState = State::Error;
                     }
@@ -234,6 +252,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to home cutter.");
                         _logWithState("Failed to call planning service for cutter homing.");
                         currentState = State::Error;
                     }
@@ -272,6 +291,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to move to pregrasp.");
                         _logWithState("Failed to call planning service for parallel move to pregrasp.");
                         currentState = State::Error;
                     }
@@ -310,6 +330,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to gripper grasp.");
                         _logWithState("Failed to call planning service for gripper grasp.");
                         currentState = State::Error;
                     }
@@ -346,6 +367,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to cutter grasp.");
                         _logWithState("Failed to call planning service for cutter grasp.");
                         currentState = State::Error;
                     }
@@ -387,6 +409,7 @@ class VADERStateMachine
                     }
                     else
                     {
+                        _publishResultStatus(srv.response.success, "Failed to move to storage.");
                         _logWithState("Failed to call planning service for parallel move to storage.");
                         currentState = State::Error;
                     }
@@ -395,6 +418,7 @@ class VADERStateMachine
                 case State::Done:
                 {
                     // ROS_INFO("Done");
+                    _publishResultStatus(vader_msgs::HarvestResult::RESULT_SUCCESS, "Success.");
                     _logWithState("Done");
                     break;
                 }

src/FVD_simulation_director.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+import os
+import rospy
+from vader_msgs.msg import HarvestResult
+import subprocess
+import threading
+import signal
+import matplotlib.pyplot as plt
+from collections import Counter
+
+class SimulationDirector:
+    def __init__(self):
+        rospy.init_node('harvest_status_listener', anonymous=True)
+        self.sub = rospy.Subscriber('/harvest_status', HarvestResult, self.callback)
+        self.message_received = threading.Event()
+        self.process = None
+        self.received_msg = None
+        self.run_count = 0
+        self.max_runs = 2
+
+        self.results = []  # Will hold result codes (e.g., 0 for failure, 1 for success)
+        self.reasons = []  # Will hold reasons for failure, etc.
+    def callback(self, msg):
+        self.received_msg = msg
+        self.results.append(self.received_msg.result)
+        self.reasons.append(self.received_msg.reason)
+        rospy.loginfo("Message received on /harvest_status: %i, failure reason (if any): %s", self.received_msg.result, self.received_msg.reason)
+        self.message_received.set()
+
+    def run(self):
+        pepper_x = 1.0 #m
+        pepper_y = 0.15 #m 
+        pepper_z = 0.4 #m
+
+        while not rospy.is_shutdown() and self.run_count < self.max_runs:
+            self.message_received.clear()
+            rospy.loginfo("Starting run %i", self.run_count)
+            # self.process = subprocess.Popen(['rosrun', 'vader_hri', 'dummy_sim.py'])# 'SVD_sim_randomized_launcher.py'])
+
+
+            self.process = subprocess.Popen(
+                ['rosrun', 'vader_hri', 'SVD_sim_randomized_launcher.py', '--x', str(pepper_x), '--y', str(pepper_y), '--z', str(pepper_z)],
+                preexec_fn=os.setsid
+            )
+
+            # Wait until a message is received
+            self.message_received.wait()
+            
+            # Print the message
+            # rospy.loginfo("Received message: %i", self.received_msg)
+            
+            # Send Ctrl+C (SIGINT) to subprocess
+            rospy.loginfo("Terminating subprocess...")
+
+
+            os.killpg(os.getpgid(self.process.pid), signal.SIGINT)
+            self.process.wait()  # Wait for subprocess to exit
+            
+            self.run_count += 1
+        self.plot_results(pepper_x, pepper_y, pepper_z)
+
+    def plot_results(self, x, y, z):
+        # Bin according to success (1) / failure (0)
+        success_count = self.results.count(1)
+        failure_count = self.run_count - success_count
+        # labels = ['Success', 'Failure']
+        # counts = [success_count, failure_count]
+
+        print("%i out of %i runs succeeded" % (success_count, self.run_count))
+
+        # plt.figure()
+        # plt.bar(labels, counts)
+        # plt.title('Success vs Failure Count')
+        # plt.show()
+
+        # Plot reasons distribution if needed
+        title = 'Failure Reason Distribution for pepper (%.2f, %.2f, %.2f)' % (x, y, z)
+        reason_counter = Counter(self.reasons)
+        plt.figure()
+        plt.bar(reason_counter.keys(), reason_counter.values())
+        plt.title(title)
+        plt.ylabel('Count')
+        plt.xlabel('Reasons')
+        plt.xticks(rotation=45)
+        plt.show()
+if __name__ == '__main__':
+    listener = SimulationDirector()
+    listener.run()

src/SVD_sim_randomized_launcher.py

@@ -7,6 +7,7 @@
 from geometry_msgs.msg import Pose
 from tf.transformations import quaternion_from_euler
 import rospy
+import argparse
 
 PREFIX="clear; roslaunch vader_hri vader_fvd_dualsim.launch "
 
@@ -16,7 +17,7 @@
     # [0.2, -0.5, 0.5],
     # [0.5, -0.6, 0.4],
     # [0.35, -0.55, 0.65],
-    [0.9, 0.25, 0.4]
+    # [0.9, 0.25, 0.4]
 ]
 
 def randomize_pepper(pepper_base_pose):
@@ -119,26 +120,13 @@ def generate_and_spawn():
 
 
 if __name__ == "__main__":
-    generate_and_spawn()
+    parser = argparse.ArgumentParser(description='Launch simulation with pepper pose.')
+    parser.add_argument('--x', type=float, default=0.0, help='X coordinate of pose')
+    parser.add_argument('--y', type=float, default=0.0, help='Y coordinate of pose')
+    parser.add_argument('--z', type=float, default=0.0, help='Z coordinate of pose')
 
-# #!/usr/bin/env python3
-# import random
+    args = parser.parse_args()
 
-# PREFIX="clear; roslaunch vader_hri vader_fvd_dualsim.launch "
-
-# def generate_poses():
-#     # Generate random pose
-#     pose = {
-#         "x": round(random.uniform(0.45, 0.55), 3), #depth from arm reach
-#         "y": round(random.uniform(0.35, 0.55), 3), #left from x-axis
-#         "z": round(random.uniform(0.4, 0.6), 3),
-#         "roll": round(random.uniform(-0.5, 0.5), 3),
-#         "pitch": round(random.uniform(-0.5, 0.5), 3)
-#     }
-
-#     arg_sim_pepper_pose = f"sim_pepper_pose:=\"{pose['x']} {pose['y']} {pose['z']} {pose['roll']} {pose['pitch']} 0.0\""
-
-#     print(PREFIX + arg_sim_pepper_pose)
-
-# if __name__ == "__main__":
-#     generate_poses()
+    pose = [args.x, args.y, args.z]
+    pepper_base_poses.append(pose)
+    generate_and_spawn()

src/dummy_sim.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python3
+import rospy
+from vader_msgs.msg import HarvestResult
+import time
+import random
+
+def publisher_node():
+    rospy.init_node('simulation_result_publisher')
+    pub = rospy.Publisher('/harvest_status', HarvestResult, queue_size=10)
+    
+    # Wait for 3 seconds before publishing
+    time.sleep(.1)
+    
+    msg = HarvestResult()
+    msg.result = random.randint(0, 10)
+    msg.reason = "testing" + str(msg.result)
+    
+    rospy.loginfo("Publishing HarvestResult message with result=%d", msg.result)
+    pub.publish(msg)
+    
+    rospy.spin()  # Keep the node alive to allow message delivery
+
+if __name__ == '__main__':
+    try:
+        publisher_node()
+    except rospy.ROSInterruptException:
+        pass