GetTrajectory

Service Definition

# Request
geometry_msgs/PoseStamped    robot_goal
int64[]                      agents_ids
---
# Response
bool                         success
string                       message
geometry_msgs/Twist          cmd_vel
Trajectory                   robot_trajectory
AgentTrajectoryArray        human_trajectories

Request Fields

  • robot_goal (geometry_msgs/PoseStamped)

    Goal pose for the robot.

  • agents_ids (int64[])

    Array of agent IDs to consider in trajectory planning.

Response Fields

  • success (bool)

    Indicates if the trajectory generation was successful.

  • message (string)

    Status message or error description.

  • cmd_vel (geometry_msgs/Twist)

    Computed velocity command for the robot.

  • robot_trajectory (Trajectory)

    Planned trajectory for the robot.

  • human_trajectories (AgentTrajectoryArray)

    Predicted trajectories for the human agents.

Example Usage

# Python
import rospy
from cohan_msgs.srv import GetTrajectory
from geometry_msgs.msg import PoseStamped

# Create service proxy
get_trajectory = rospy.ServiceProxy('get_trajectory', GetTrajectory)

# Create request
goal = PoseStamped()
goal.header.frame_id = "map"
goal.header.stamp = rospy.Time.now()
# Set goal pose...

agents = [1, 2]  # Example agent IDs

try:
    # Call service
    response = get_trajectory(goal, agents)

    if response.success:
        # Process trajectories
        robot_traj = response.robot_trajectory
        human_trajs = response.human_trajectories
    else:
        rospy.logwarn("Failed to get trajectory: %s", response.message)
except rospy.ServiceException as e:
    rospy.logerr("Service call failed: %s", e)