Optimize

Service Definition

# Get optimized timed elastic bands for given agents and robot plans

# Request
nav_msgs/Path              robot_plan
AgentPathArray            agent_plan_array
int64[]                   agents_ids
---
# Response
bool                      success
string                    message
geometry_msgs/Twist       cmd_vel
Trajectory                robot_trajectory
AgentTrajectoryArray     human_trajectories

Request Fields

  • robot_plan (nav_msgs/Path)

    Initial path plan for the robot.

  • agent_plan_array (AgentPathArray)

    Array of initial path plans for the agents.

  • agents_ids (int64[])

    Array of agent IDs to consider in the optimization.

Response Fields

  • success (bool)

    Indicates if the optimization was successful.

  • message (string)

    Status message or error description.

  • cmd_vel (geometry_msgs/Twist)

    Optimized velocity command for the robot.

  • robot_trajectory (Trajectory)

    Optimized trajectory for the robot.

  • human_trajectories (AgentTrajectoryArray)

    Optimized trajectories for the human agents.

Example Usage

# Python
import rospy
from cohan_msgs.srv import Optimize
from nav_msgs.msg import Path
from cohan_msgs.msg import AgentPathArray

# Create service proxy
optimize = rospy.ServiceProxy('optimize', Optimize)

# Create request
robot_plan = Path()
# Set robot plan...

agent_plans = AgentPathArray()
# Set agent plans...

agents = [1, 2]  # Example agent IDs

try:
    # Call service
    response = optimize(robot_plan, agent_plans, agents)

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