moveit_pro::behaviors::internal Namespace Reference

Classes
class	LatencyWindow
	Seam-lead estimate over the last kLatencyWindowSize inference round trips: their max, floored. More...
class	MPCBehaviorBase
	All non-template implementation for the templated MPCBehaviorBase class. More...
struct	Sam2AutomaskModelPorts
struct	Sam3ModelPorts

Enumerations
enum class	PolicyResponseOutcome { kChunkProduced , kEndOfRun , kFailed }

Functions
PolicyResponseOutcome	classifyPolicyResponse (std::uint8_t status)
	Classify a GetActionChunk response status into the run outcome it demands.
std::string	policyResponseFailureMessage (const moveit_pro_ml_msgs::srv::GetActionChunk::Response &response)
	Render a response classified kFailed as the run's failure reason.
bool	exceedsPositionLimitTolerance (double position, bool position_bounded, double min_position, double max_position, double tolerance_fraction)
	Report whether a position command lies farther outside a joint's position limits than a tolerated fraction of its position range.
template<typename Lockable , typename StopTransition >
void	stopRecordingUnderLock (Lockable &mutex, StopTransition &&set_stopped)
	Performs the stop transition while holding the mutex shared with trajectory mutation.
template<typename Lockable , typename StopPredicate , typename TrajectoryMutation >
bool	mutateTrajectoryUnlessStopped (Lockable &mutex, StopPredicate &&is_stopped, TrajectoryMutation &&mutate_trajectory)
	Mutates the trajectory only while the stop predicate is false under the shared mutex.
tl::expected< visualization_msgs::msg::MarkerArray, std::string >	makeRejectedWaypointMarkers (const moveit_pro::base::planning_scene::PlanningScene &planning_scene, const moveit_pro::base::JointModelGroup &group, const std::vector< Eigen::VectorXd > &checked_path, int path_index, const moveit_pro::base::collision_detection::CollisionResult::ContactMap &contacts)
	Markers for the waypoint a collision check refused.
tl::expected< moveit_pro::hand_eye_calibration::CameraIntrinsics, std::string >	toCameraIntrinsics (const sensor_msgs::msg::CameraInfo &camera_info)
	Extract the pinhole intrinsics the calibration library consumes from a CameraInfo message.
tl::expected< std::vector< moveit_pro::hand_eye_calibration::PosePair >, std::string >	collectPosePairs (const std::vector< moveit_pro::hand_eye_calibration::CalibrationSample > &samples, const std::string &camera_name, const moveit_pro::hand_eye_calibration::CameraIntrinsics &intrinsics)
	Distill one camera's pose pairs from the collected samples.

Variables
constexpr std::size_t	kLatencyWindowSize = 5

Enumeration Type Documentation

PolicyResponseOutcome

enum class moveit_pro::behaviors::internal::PolicyResponseOutcome

strong

How a policy response's status maps onto the run: execute the chunk, end the run as complete, or fail it.

Enumerator
kChunkProduced
kEndOfRun
kFailed

Function Documentation

classifyPolicyResponse()

PolicyResponseOutcome moveit_pro::behaviors::internal::classifyPolicyResponse

(

std::uint8_t

status

)

Classify a GetActionChunk response status into the run outcome it demands.

CHUNK_PRODUCED executes the chunk and NO_MORE_ACTIONS ends the run as complete; everything else fails the run: ERROR, UNKNOWN (the server never set the field), and any value the contract does not define, so only an explicit NO_MORE_ACTIONS may end a run as complete.

Parameters

status

The response's status field.

Returns

The outcome the caller must apply.

collectPosePairs()

tl::expected< std::vector< moveit_pro::hand_eye_calibration::PosePair >, std::string > moveit_pro::behaviors::internal::collectPosePairs	(	const std::vector< moveit_pro::hand_eye_calibration::CalibrationSample > &	samples,
		const std::string &	camera_name,
		const moveit_pro::hand_eye_calibration::CameraIntrinsics &	intrinsics
	)

Distill one camera's pose pairs from the collected samples.

Samples where the named camera did not usably see the board, holding no detections or too few corners for a board pose, are skipped, so each camera may use its own subset of the collected configurations. For every remaining sample the board pose is derived by PnP on the stored corner observations.

Parameters

samples	All collected calibration samples.
camera_name	The camera to solve for.
intrinsics	That camera's intrinsics.

Returns

One pose pair per usable sample, or an error when no sample holds usable detections for the camera or a board pose cannot be estimated.

exceedsPositionLimitTolerance()

bool moveit_pro::behaviors::internal::exceedsPositionLimitTolerance	(	double	position,
		bool	position_bounded,
		double	min_position,
		double	max_position,
		double	tolerance_fraction
	)

Report whether a position command lies farther outside a joint's position limits than a tolerated fraction of its position range.

Parameters

position	The commanded position.
position_bounded	Whether the joint declares position limits.
min_position	Lower position limit, used only when position_bounded is true.
max_position	Upper position limit, used only when position_bounded is true.
tolerance_fraction	Fraction of the position range (max_position - min_position) tolerated past either limit. Must be non-negative; a negative fraction inverts the band.

Returns

True when the joint is bounded and the position lies more than the tolerated band below min_position or above max_position; false otherwise, including for an unbounded joint.

makeRejectedWaypointMarkers()

tl::expected< visualization_msgs::msg::MarkerArray, std::string > moveit_pro::behaviors::internal::makeRejectedWaypointMarkers	(	const moveit_pro::base::planning_scene::PlanningScene &	planning_scene,
		const moveit_pro::base::JointModelGroup &	group,
		const std::vector< Eigen::VectorXd > &	checked_path,
		int	path_index,
		const moveit_pro::base::collision_detection::CollisionResult::ContactMap &	contacts
	)

Markers for the waypoint a collision check refused.

Draws the refused configuration as a translucent robot, followed by spheres at the contacts that refused it. Internal: exposed so the waypoint selection is testable without running a collision check or a ROS publisher, and not covered by the API-compatibility guarantee.

Parameters

planning_scene	Scene the path was checked against. Its current state seeds the drawn pose, so joints outside group and attached bodies keep their scene values.
group	Planning group whose active joints checked_path describes.
checked_path	Waypoints exactly as handed to the collision check — the downsampled path, not the input trajectory, since that is what path_index counts.
path_index	Waypoint the collision check refused.
contacts	Contacts the check reported.

Returns

The markers, or an error when path_index falls outside checked_path. Neither the pose nor the spheres are returned on error: both describe one waypoint, so an index that does not identify one leaves nothing meaningful to draw.

mutateTrajectoryUnlessStopped()

template<typename Lockable , typename StopPredicate , typename TrajectoryMutation >

bool moveit_pro::behaviors::internal::mutateTrajectoryUnlessStopped	(	Lockable &	mutex,
		StopPredicate &&	is_stopped,
		TrajectoryMutation &&	mutate_trajectory
	)

Mutates the trajectory only while the stop predicate is false under the shared mutex.

Returns

True when the mutation ran; false when recording had already stopped.

policyResponseFailureMessage()

std::string moveit_pro::behaviors::internal::policyResponseFailureMessage

(

const moveit_pro_ml_msgs::srv::GetActionChunk::Response &

response

)

Render a response classified kFailed as the run's failure reason.

An unset or contract-undefined status gets its own text naming the adapter-side bug; ERROR reports the response's message as the reason.

Parameters

response

The policy response that failed the run.

Returns

The operator-facing failure reason.

stopRecordingUnderLock()

template<typename Lockable , typename StopTransition >

void moveit_pro::behaviors::internal::stopRecordingUnderLock	(	Lockable &	mutex,
		StopTransition &&	set_stopped
	)

Performs the stop transition while holding the mutex shared with trajectory mutation.

toCameraIntrinsics()

tl::expected< moveit_pro::hand_eye_calibration::CameraIntrinsics, std::string > moveit_pro::behaviors::internal::toCameraIntrinsics

(

const sensor_msgs::msg::CameraInfo &

camera_info

)

Extract the pinhole intrinsics the calibration library consumes from a CameraInfo message.

Uses the K matrix and the raw distortion coefficients, matching how the detections are made on the unrectified image.

Parameters

camera_info

The camera's intrinsics message.

Returns

The library intrinsics, or an error when the focal lengths are not positive, the distortion model is not plumb_bob, rational_polynomial, or empty, the coefficient count does not match the declared model, or coefficients are carried with no declared model.

Variable Documentation

kLatencyWindowSize

constexpr std::size_t moveit_pro::behaviors::internal::kLatencyWindowSize = 5

inlineconstexpr