motion_planning
-
class MotionPlanningImpl
Implement the core motion planning logic shared across ROS 2 service handlers.
MotionPlanningImpl owns the server pool, the workcell client, and all configuration loaded at startup. The ROS 2 node (MotionPlanningNode) holds a shared_ptr to this object and delegates actual computation to it.
Public Functions
-
MotionPlanningImpl()
Construct a default MotionPlanningImpl with uninitialised internals.
-
~MotionPlanningImpl()
Destroy the MotionPlanningImpl and release all owned resources.
Initialise the implementation with configuration files and a ROS 2 node.
Loads arm planning, motion planning, OPW kinematics, and inverse-dynamics configuration from the given paths, then starts the server pool and connects to the workcell description service.
- Parameters:
node – Shared ROS 2 node used for service clients and logging.
pathToArmPlanningConfig – File path to the arm planning JSON/YAML config.
pathToMotionPlanningConfig – File path to the motion planning JSON/YAML config.
pathToOpwParams – File path to the OPW kinematics parameter file.
pathToInverseDynamicsConfig – File path to the inverse dynamics config.
- Returns:
true on success, false if any configuration step fails.
-
bool GetIkRefJointState(const core_types::JointState_t &refJointState, core_types::JointState_t &outRefJointState)
Resolve the IK reference joint state closest to a given reference.
- Parameters:
refJointState – Input reference joint state used to seed the IK.
outRefJointState – Output joint state that satisfies the IK reference.
- Returns:
true if a valid reference state was found, false otherwise.
-
bool ReturnServer(std::unique_ptr<core_motion_planning::Server> &server)
Return a server instance back to the pool after use.
- Parameters:
server – Unique pointer to the server being returned; will be moved-from.
- Returns:
true if the server was successfully returned, false otherwise.
-
int UpdateObjectsFromWorkcell()
Fetch the latest collision objects from the workcell and update internal state.
- Returns:
0 on success, or a non-zero error code on failure.
-
int WaitAndObtainServer(std::unique_ptr<core_motion_planning::Server> &server, const core_motion_planning::ServerType serverType, const bool update_objects = true)
Block until a planning server of the requested type is available, then acquire it.
Optionally refreshes workcell objects before returning the server.
- Parameters:
server – Output unique pointer that receives the acquired server.
serverType – The type of planning server to acquire.
update_objects – If true, call UpdateObjectsFromWorkcell() before returning.
- Returns:
0 on success, or a non-zero error code if acquisition fails.
-
uint32_t GetQosServiceQueueDepth() const
Return the configured QoS service queue depth.
- Returns:
Queue depth as a 32-bit unsigned integer.
-
bool TrajectoryToJointStates(const trajectory_msgs::msg::JointTrajectory &trajectory, std::vector<sensor_msgs::msg::JointState> &joint_states)
Convert a JointTrajectory message into a sequence of JointState messages.
Each waypoint in
trajectoryis mapped to one JointState element injoint_states.- Parameters:
trajectory – Input joint trajectory to convert.
joint_states – Output vector populated with one JointState per waypoint.
- Returns:
true on success, false if the trajectory is malformed.
-
MotionPlanningImpl()