self_mask_urdf_robot.h
Go to the documentation of this file.
00001 #ifndef JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_
00002 #define JSK_PCL_ROS_SELF_MASK_URDF_ROBOT_
00003 
00004 #include <robot_self_filter/self_mask.h>
00005 #include <kdl/tree.hpp>
00006 #include <kdl/chainjnttojacsolver.hpp>
00007 #include <kdl/chainfksolverpos_recursive.hpp>
00008 #include <kdl_parser/kdl_parser.hpp>
00009 #include <kdl_conversions/kdl_msg.h>
00010 #include <tf_conversions/tf_kdl.h>
00011 #include <tf/transform_broadcaster.h>
00012 #include <sensor_msgs/JointState.h>
00013 #include <geometry_msgs/PoseStamped.h>
00014 
00015 namespace robot_self_filter
00016 {
00017   class SelfMaskUrdfRobot : public SelfMask<pcl::PointXYZ>
00018   {
00019   public:
00020   SelfMaskUrdfRobot(tf::TransformListener &tfl,
00021                     tf::TransformBroadcaster &tfb,
00022                     const std::vector<LinkInfo> &links,
00023                     const urdf::Model &urdf_model,
00024                     std::string root_link_id = "BODY",
00025                     std::string world_frame_id = "map",
00026                     bool set_foot_pose = false)
00027     : SelfMask<pcl::PointXYZ>(tfl, links),
00028       tf_broadcaster_(tfb),
00029       urdf_model_(urdf_model),
00030       root_link_id_(root_link_id),
00031       world_frame_id_(world_frame_id)
00032     {
00033       ros::NodeHandle pnh("~");
00034       pnh.param("publish_tf", publish_tf_, false);
00035       initKdlConfigure();
00036     }
00037 
00038     bool initKdlConfigure()
00039     {
00040       // generate kdl tree
00041       if(!kdl_parser::treeFromUrdfModel(urdf_model_, tree_)) {
00042         ROS_FATAL("Failed to load robot_description");
00043         return false;
00044       }
00045       // generate kdl tree
00046       for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
00047         std::string name = bodies_[i].name;
00048         tree_.getChain(root_link_id_, name, chain_map_[name]);
00049         for(size_t j = 0; j < chain_map_[name].getNrOfSegments(); j++) {
00050           ROS_DEBUG_STREAM("kdl_chain(" << j << ") "
00051                            << chain_map_[name].getSegment(j).getJoint().getName().c_str());
00052         }
00053       }
00054       return true;
00055     }
00056 
00057     void updateChain(std::map<std::string, double>& joint_angles,
00058                      KDL::Chain& chain,
00059                      tf::Pose& output_pose)
00060     {
00061       // solve forward kinematics
00062       KDL::JntArray jnt_pos(chain.getNrOfJoints());
00063       for (int i = 0, j = 0; i < chain.getNrOfSegments(); i++) {
00064         std::string joint_name = chain.getSegment(i).getJoint().getName();
00065         if (joint_angles.find(joint_name) != joint_angles.end()) {
00066           jnt_pos(j++) = joint_angles[joint_name];
00067         }
00068       }
00069       KDL::ChainFkSolverPos_recursive fk_solver(chain);
00070       KDL::Frame pose;
00071       if (fk_solver.JntToCart(jnt_pos, pose) < 0) {
00072         ROS_FATAL("Failed to compute FK");
00073       }
00074       tf::poseKDLToTF(pose, output_pose);
00075     }
00076 
00077     void updateRobotModel(std::map<std::string, double>& joint_angles, const tf::Pose& root_pose)
00078     {
00079       // for debug
00080       /* std::map<std::string, double> joint_angles; */
00081       /* for(std::map<std::string, boost::shared_ptr<urdf::Joint> >::iterator it = urdf_model_.joints_.begin(); it != urdf_model_.joints_.end(); ++it) { */
00082       /*   joint_angles[it->first] = 0.0; */
00083       /* } */
00084 
00085       for(std::map<std::string, double>::iterator it = joint_angles.begin(); it != joint_angles.end(); ++it) {
00086         ROS_DEBUG_STREAM("joint " << it->first << " : " << it->second);
00087       }
00088 
00089       for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
00090         std::string name = bodies_[i].name;
00091         updateChain(joint_angles, chain_map_[name], pose_map_[name]);
00092         pose_map_[name] = root_pose * pose_map_[name];
00093       }
00094 
00095       // publish tf of all links
00096       if(publish_tf_) {
00097         std::vector<tf::StampedTransform> link_transforms;
00098         for(unsigned int i = 0 ; i < bodies_.size() ; i++) {
00099           std::string name = bodies_[i].name;
00100           tf::Pose link_trans = pose_map_[name];
00101           ros::Time stamp = ros::Time::now();
00102           link_transforms.push_back(tf::StampedTransform(link_trans, stamp, world_frame_id_, "collision_detector/"+name));
00103           ROS_DEBUG_STREAM("link trans [" << name <<
00104                            "] pos=( " << link_trans.getOrigin().getX() << ", " << link_trans.getOrigin().getY() << ", " << link_trans.getOrigin().getZ() <<
00105                            ")  rot=( " << link_trans.getRotation().getW() << ", " << link_trans.getRotation().getX() << ", " << link_trans.getRotation().getY() << ", " << link_trans.getRotation().getZ() << ")");
00106         }
00107         tf_broadcaster_.sendTransform(link_transforms);
00108       }
00109     }
00110 
00111     void assumeFrameFromJointAngle(const sensor_msgs::JointState& joint, const geometry_msgs::PoseStamped& pose)
00112     {
00113       std::map<std::string, double> joint_angles;
00114       for (size_t i = 0; i < joint.name.size(); i++) {
00115         joint_angles[joint.name[i]] = joint.position[i];
00116       }
00117       tf::Pose tf_pose;
00118       tf::poseMsgToTF(pose.pose, tf_pose);
00119       updateRobotModel(joint_angles, tf_pose);
00120 
00121       // update pose of all links
00122       for(int i = 0; i < bodies_.size(); i++) {
00123         std::string name = bodies_[i].name;
00124         // pose_map_ should be the transformation from world frame to link frame.
00125         bodies_[i].body->setPose(pose_map_[name] * bodies_[i].constTransf);
00126         bodies_[i].unscaledBody->setPose(pose_map_[name] * bodies_[i].constTransf);
00127       }
00128 
00129       computeBoundingSpheres();
00130     }
00131 
00132   protected:
00133     urdf::Model urdf_model_;
00134     KDL::Tree tree_;
00135     std::map<std::string, KDL::Chain> chain_map_;
00136     std::map<std::string, tf::Pose> pose_map_;
00137     tf::TransformBroadcaster &tf_broadcaster_;
00138     std::string world_frame_id_;
00139     std::string root_link_id_;
00140     bool publish_tf_;
00141   };
00142 
00143 }
00144 
00145 #endif


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:45