timeandpath

#!/usr/bin/env python

import rospy
import moveit_commander
import time
import math
import sys

def compute_trajectory_length(trajectory):
    """
    Compute the length of the trajectory.
    :param trajectory: RobotTrajectory object from MoveIt
    :return: Total length of the trajectory
    """
    total_length = 0.0
    # Extract trajectory points
    for i in range(1, len(trajectory.joint_trajectory.points)):
        prev_point = trajectory.joint_trajectory.points[i - 1].positions
        current_point = trajectory.joint_trajectory.points[i].positions
        # Compute Euclidean distance between joint positions
        distance = math.sqrt(sum((c - p) ** 2 for c, p in zip(current_point, prev_point)))
        total_length += distance
    return total_length

def main():
    # Initialize the ROS node
    rospy.init_node("trajectory_planning_node", anonymous=True)

    # Initialize MoveIt commander
    moveit_commander.roscpp_initialize(sys.argv)

    # Initialize the robot and planning group
    robot = moveit_commander.RobotCommander()
    group_name = "arm_group"  # Update this to your robot group name
    group = moveit_commander.MoveGroupCommander(group_name)

    # Define the start and goal positions
    start_pose = group.get_current_pose().pose  # Current pose as the start position
    goal_pose = start_pose
    goal_pose.position.x += 0.1  # Adjust the goal position in X direction
    goal_pose.position.y += 0.1  # Adjust the goal position in Y direction
    goal_pose.position.z += 0.1  # Adjust the goal position in Z direction

    # Set the goal pose
    group.set_pose_target(goal_pose)

    # Start timing for planning (NEW: Added timing for planning phase)
    planning_start_time = time.time()  # Start timing before planning starts

    # Plan the trajectory
    plan = group.plan()

    # End timing for planning (NEW: Added timing for planning phase)
    planning_end_time = time.time()  # End timing after planning ends

    # Check if planning was successful
    if plan and plan[0]:  # Plan is a tuple, first element contains the trajectory
        trajectory = plan[1]  # The trajectory is in the second element

        # Compute the trajectory length (Same as before)
        trajectory_length = compute_trajectory_length(trajectory)

        # Compute the planning time (NEW: Added computation for planning time)
        planning_time = planning_end_time - planning_start_time  # Planning time = end - start

        rospy.loginfo(f"Trajectory planning successful!")
        rospy.loginfo(f"Planning time: {planning_time:.4f} seconds")  # NEW: Log planning time
        rospy.loginfo(f"Trajectory length: {trajectory_length:.4f}")  # Same as before

        # Start timing for execution (NEW: Added timing for execution phase)
        execution_start_time = time.time()  # Start timing before execution starts

        # Execute the trajectory
        group.execute(trajectory, wait=True)

        # End timing for execution (NEW: Added timing for execution phase)
        execution_end_time = time.time()  # End timing after execution ends

        # Compute the execution time and total time (NEW: Added total time calculation)
        execution_time = execution_end_time - execution_start_time  # Execution time = end - start
        total_time = planning_time + execution_time  # Total time = planning time + execution time

        rospy.loginfo(f"Execution time: {execution_time:.4f} seconds")  # NEW: Log execution time
        rospy.loginfo(f"Total time (planning + execution): {total_time:.4f} seconds")  # NEW: Log total time

    else:
        rospy.logerr("Trajectory planning failed!")

    # Shut down MoveIt commander
    moveit_commander.roscpp_shutdown()

if __name__ == "__main__":
    main()