1 #ifndef JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_ 2 #define JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_ 4 #include <robot_self_filter/self_mask.h> 5 #include <kdl/tree.hpp> 6 #include <kdl/chainjnttojacsolver.hpp> 7 #include <kdl/chainfksolverpos_recursive.hpp> 12 #include <sensor_msgs/JointState.h> 13 #include <geometry_msgs/PoseStamped.h> 22 const std::vector<LinkInfo> &links,
24 std::string root_link_id =
"BODY",
25 std::string world_frame_id =
"map",
26 bool set_foot_pose =
false)
27 : SelfMask<
pcl::PointXYZ>(tfl, links),
42 ROS_FATAL(
"Failed to load robot_description");
46 for(
unsigned int i = 0 ; i < bodies_.size() ; i++) {
47 std::string name = bodies_[i].name;
49 for(
size_t j = 0; j <
chain_map_[name].getNrOfSegments(); j++) {
65 if (joint_angles.find(joint_name) != joint_angles.end()) {
66 jnt_pos(j++) = joint_angles[joint_name];
71 if (fk_solver.
JntToCart(jnt_pos, pose) < 0) {
85 for(std::map<std::string, double>::iterator it = joint_angles.begin(); it != joint_angles.end(); ++it) {
89 for(
unsigned int i = 0 ; i < bodies_.size() ; i++) {
90 std::string name = bodies_[i].name;
97 std::vector<tf::StampedTransform> link_transforms;
98 for(
unsigned int i = 0 ; i < bodies_.size() ; i++) {
99 std::string name = bodies_[i].name;
113 std::map<std::string, double> joint_angles;
114 for (
size_t i = 0; i < joint.name.size(); i++) {
115 joint_angles[joint.name[i]] = joint.position[i];
122 for(
int i = 0; i < bodies_.size(); i++) {
123 std::string name = bodies_[i].name;
125 bodies_[i].body->setPose(
pose_map_[name] * bodies_[i].constTransf);
126 bodies_[i].unscaledBody->setPose(
pose_map_[name] * bodies_[i].constTransf);
129 computeBoundingSpheres();
std::map< std::string, tf::Pose > pose_map_
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
const Segment & getSegment(unsigned int nr) const
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
unsigned int getNrOfSegments() const
std::string getName(void *handle)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
TFSIMD_FORCE_INLINE const tfScalar & getW() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
SelfMaskUrdfRobot(tf::TransformListener &tfl, tf::TransformBroadcaster &tfb, const std::vector< LinkInfo > &links, const urdf::Model &urdf_model, std::string root_link_id="BODY", std::string world_frame_id="map", bool set_foot_pose=false)
std::string root_link_id_
const std::string & getName() const
void poseKDLToTF(const KDL::Frame &k, tf::Pose &t)
std::map< std::string, KDL::Chain > chain_map_
tf::TransformBroadcaster & tf_broadcaster_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const Joint & getJoint() const
unsigned int getNrOfJoints() const
std::string world_frame_id_
#define ROS_DEBUG_STREAM(args)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
void updateChain(std::map< std::string, double > &joint_angles, KDL::Chain &chain, tf::Pose &output_pose)
void updateRobotModel(std::map< std::string, double > &joint_angles, const tf::Pose &root_pose)
void assumeFrameFromJointAngle(const sensor_msgs::JointState &joint, const geometry_msgs::PoseStamped &pose)