self_mask_urdf_robot.h
Go to the documentation of this file.
1 #ifndef JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_
2 #define JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_
3 
4 #include <robot_self_filter/self_mask.h>
5 #include <kdl/tree.hpp>
6 #include <kdl/chainjnttojacsolver.hpp>
7 #include <kdl/chainfksolverpos_recursive.hpp>
10 #include <tf_conversions/tf_kdl.h>
12 #include <sensor_msgs/JointState.h>
13 #include <geometry_msgs/PoseStamped.h>
14 
15 namespace robot_self_filter
16 {
17  class SelfMaskUrdfRobot : public SelfMask<pcl::PointXYZ>
18  {
19  public:
22  const std::vector<LinkInfo> &links,
23  const urdf::Model &urdf_model,
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),
28  tf_broadcaster_(tfb),
29  urdf_model_(urdf_model),
30  root_link_id_(root_link_id),
31  world_frame_id_(world_frame_id)
32  {
33  ros::NodeHandle pnh("~");
34  pnh.param("publish_tf", publish_tf_, false);
36  }
37 
39  {
40  // generate kdl tree
42  ROS_FATAL("Failed to load robot_description");
43  return false;
44  }
45  // generate kdl tree
46  for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
47  std::string name = bodies_[i].name;
48  tree_.getChain(root_link_id_, name, chain_map_[name]);
49  for(size_t j = 0; j < chain_map_[name].getNrOfSegments(); j++) {
50  ROS_DEBUG_STREAM("kdl_chain(" << j << ") "
51  << chain_map_[name].getSegment(j).getJoint().getName().c_str());
52  }
53  }
54  return true;
55  }
56 
57  void updateChain(std::map<std::string, double>& joint_angles,
58  KDL::Chain& chain,
59  tf::Pose& output_pose)
60  {
61  // solve forward kinematics
62  KDL::JntArray jnt_pos(chain.getNrOfJoints());
63  for (int i = 0, j = 0; i < chain.getNrOfSegments(); i++) {
64  std::string joint_name = chain.getSegment(i).getJoint().getName();
65  if (joint_angles.find(joint_name) != joint_angles.end()) {
66  jnt_pos(j++) = joint_angles[joint_name];
67  }
68  }
69  KDL::ChainFkSolverPos_recursive fk_solver(chain);
71  if (fk_solver.JntToCart(jnt_pos, pose) < 0) {
72  ROS_FATAL("Failed to compute FK");
73  }
74  tf::poseKDLToTF(pose, output_pose);
75  }
76 
77  void updateRobotModel(std::map<std::string, double>& joint_angles, const tf::Pose& root_pose)
78  {
79  // for debug
80  /* std::map<std::string, double> joint_angles; */
81  /* for(std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_model_.joints_.begin(); it != urdf_model_.joints_.end(); ++it) { */
82  /* joint_angles[it->first] = 0.0; */
83  /* } */
84 
85  for(std::map<std::string, double>::iterator it = joint_angles.begin(); it != joint_angles.end(); ++it) {
86  ROS_DEBUG_STREAM("joint " << it->first << " : " << it->second);
87  }
88 
89  for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
90  std::string name = bodies_[i].name;
91  updateChain(joint_angles, chain_map_[name], pose_map_[name]);
92  pose_map_[name] = root_pose * pose_map_[name];
93  }
94 
95  // publish tf of all links
96  if(publish_tf_) {
97  std::vector<tf::StampedTransform> link_transforms;
98  for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
99  std::string name = bodies_[i].name;
100  tf::Pose link_trans = pose_map_[name];
102  link_transforms.push_back(tf::StampedTransform(link_trans, stamp, world_frame_id_, "collision_detector/"+name));
103  ROS_DEBUG_STREAM("link trans [" << name <<
104  "] pos=( " << link_trans.getOrigin().getX() << ", " << link_trans.getOrigin().getY() << ", " << link_trans.getOrigin().getZ() <<
105  ") rot=( " << link_trans.getRotation().getW() << ", " << link_trans.getRotation().getX() << ", " << link_trans.getRotation().getY() << ", " << link_trans.getRotation().getZ() << ")");
106  }
107  tf_broadcaster_.sendTransform(link_transforms);
108  }
109  }
110 
111  void assumeFrameFromJointAngle(const sensor_msgs::JointState& joint, const geometry_msgs::PoseStamped& pose)
112  {
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];
116  }
117  tf::Pose tf_pose;
118  tf::poseMsgToTF(pose.pose, tf_pose);
119  updateRobotModel(joint_angles, tf_pose);
120 
121  // update pose of all links
122  for(int i = 0; i < bodies_.size(); i++) {
123  std::string name = bodies_[i].name;
124  // pose_map_ should be the transformation from world frame to link frame.
125  bodies_[i].body->setPose(pose_map_[name] * bodies_[i].constTransf);
126  bodies_[i].unscaledBody->setPose(pose_map_[name] * bodies_[i].constTransf);
127  }
128 
129  computeBoundingSpheres();
130  }
131 
132  protected:
135  std::map<std::string, KDL::Chain> chain_map_;
136  std::map<std::string, tf::Pose> pose_map_;
138  std::string world_frame_id_;
139  std::string root_link_id_;
141  };
142 
143 }
144 
145 #endif
std::map< std::string, tf::Pose > pose_map_
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
#define ROS_FATAL(...)
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
pose
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)
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 &param_name, T &param_val, const T &default_val) const
void sendTransform(const StampedTransform &transform)
const Joint & getJoint() const
unsigned int getNrOfJoints() const
#define ROS_DEBUG_STREAM(args)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
Quaternion getRotation() const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static Time now()
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)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47