37 #ifndef MOVEIT_ROBOT_STATE_CONVERSIONS_ 38 #define MOVEIT_ROBOT_STATE_CONVERSIONS_ 42 #include <moveit_msgs/RobotState.h> 43 #include <moveit_msgs/RobotTrajectory.h> 66 bool copy_attached_bodies =
true);
76 bool copy_attached_bodies =
true);
85 bool copy_attached_bodies =
true);
93 const std::vector<const AttachedBody*>& attached_bodies,
94 std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs);
120 void robotStateToStream(
const RobotState& state, std::ostream& out,
bool include_header =
true,
121 const std::string& separator =
",");
133 const std::vector<std::string>& joint_groups_ordering,
bool include_header =
true,
134 const std::string& separator =
",");
143 void streamToRobotState(RobotState& state,
const std::string& line,
const std::string& separator =
",");
Core components of MoveIt!
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
void robotStateToStream(const RobotState &state, std::ostream &out, bool include_header=true, const std::string &separator=",")
Convert a MoveIt! robot state to common separated values (CSV) on a single line that is outputted to ...
bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory &trajectory, std::size_t point_id, RobotState &state)
Convert a joint trajectory point to a MoveIt! robot state.
void robotStateToJointStateMsg(const RobotState &state, sensor_msgs::JointState &joint_state)
Convert a MoveIt! robot state to a joint state message.
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt! robot state to a robot state message.
bool jointStateToRobotState(const sensor_msgs::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt! robot state.
Main namespace for MoveIt!
void streamToRobotState(RobotState &state, const std::string &line, const std::string &separator=",")
Convert a string of joint values from a file (CSV) or input source into a RobotState.