#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/trajectory_execution_info.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <boost/tokenizer.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
#include <moveit_msgs/msg/visibility_constraint.hpp>
#include <moveit_studio_agent_msgs/msg/robot_joint_state.hpp>
#include <tl_expected/expected.hpp>
|
std::optional< moveit::task_constructor::TrajectoryExecutionInfo > | moveit_studio::helper_functions::getDefaultTrajectoryExecutionInfo (const std::string &controller_names) |
| Process a string containing a space-separated list of controller names to compose an MTC TrajectoryExecutionInfo object describing which controllers to use when executing the trajectory. More...
|
|
tl::expected< sensor_msgs::msg::JointState, std::string > | moveit_studio::helper_functions::filterJointStateForGroup (const moveit_studio_agent_msgs::msg::RobotJointState &joint_state_in, const moveit::core::RobotModelConstPtr &robot_model, const std::string &planning_group_name) |
| Create a joint state that contains the subset of joints in an input joint state that are associated with a particular planning group. More...
|
|
moveit_msgs::msg::VisibilityConstraint | moveit_studio::helper_functions::createCameraVisibilityConstraint (const std::string &camera_optical_frame, const std_msgs::msg::Header &target_header, const geometry_msgs::msg::Pose &target_pose, const double camera_field_of_view, const double target_diameter, const double sensor_z_offset) |
| Create a VisibilityConstraint to make the robot look at an object in the scene. More...
|
|
std::string | moveit_studio::helper_functions::getFullName (const moveit::task_constructor::Stage *stage) |
| Get the fully-qualified name of a MTC stage. More...
|
|
bool | moveit_studio::helper_functions::collisionValidationFunction (const moveit::core::JointModelGroup &group, const Eigen::VectorXd &goal_positions, planning_scene::PlanningScene &planning_scene) |
| Validate a joint-space configuration by checking for collisions in the given planning scene. More...
|
|