joint_state.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Aldebaran
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019 * LOCAL includes
00020 */
00021 #include "joint_state.hpp"
00022 #include "nao_footprint.hpp"
00023 
00024 /*
00025 * BOOST includes
00026 */
00027 #include <boost/foreach.hpp>
00028 #define for_each BOOST_FOREACH
00029 
00030 /*
00031 * ROS includes
00032 */
00033 #include <urdf/model.h>
00034 #include <kdl_parser/kdl_parser.hpp>
00035 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00036 
00037 namespace naoqi
00038 {
00039 
00040 namespace converter
00041 {
00042 
00043 JointStateConverter::JointStateConverter( const std::string& name, const float& frequency, const BufferPtr& tf2_buffer, const qi::SessionPtr& session ):
00044   BaseConverter( name, frequency, session ),
00045   p_motion_( session->service("ALMotion") ),
00046   tf2_buffer_(tf2_buffer)
00047 {
00048   robot_desc_ = tools::getRobotDescription( robot_ );
00049 }
00050 
00051 JointStateConverter::~JointStateConverter()
00052 {
00053   // clear all sessions
00054 }
00055 
00056 void JointStateConverter::reset()
00057 {
00058   if ( robot_desc_.empty() )
00059   {
00060     std::cout << "error in loading robot description" << std::endl;
00061     return;
00062   }
00063   urdf::Model model;
00064   model.initString( robot_desc_ );
00065   KDL::Tree tree;
00066   kdl_parser::treeFromUrdfModel( model, tree );
00067 
00068   addChildren( tree.getRootSegment() );
00069 
00070   // set mimic joint list
00071   mimic_.clear();
00072   for(std::map< std::string, boost::shared_ptr< urdf::Joint > >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++){
00073     if(i->second->mimic){
00074       mimic_.insert(make_pair(i->first, i->second->mimic));
00075     }
00076   }
00077   // pre-fill joint states message
00078   msg_joint_states_.name = p_motion_.call<std::vector<std::string> >("getBodyNames", "Body" );
00079 }
00080 
00081 void JointStateConverter::registerCallback( const message_actions::MessageAction action, Callback_t cb )
00082 {
00083   callbacks_[action] = cb;
00084 }
00085 
00086 void JointStateConverter::callAll( const std::vector<message_actions::MessageAction>& actions )
00087 {
00088   // get joint state values
00089   std::vector<double> al_joint_angles = p_motion_.call<std::vector<double> >("getAngles", "Body", true );
00090   const ros::Time& stamp = ros::Time::now();
00091 
00095   msg_joint_states_.header.stamp = stamp;
00096 
00097   // STUPID CONVERTION FROM FLOAT TO DOUBLE ARRAY --> OPTIMIZE THAT!
00098   msg_joint_states_.position = std::vector<double>( al_joint_angles.begin(), al_joint_angles.end() );
00099 
00103   // put joint states in tf broadcaster
00104   std::map< std::string, double > joint_state_map;
00105   // stupid version --> change this with std::transform c++11 ??!
00106 //  std::transform( msg_joint_states_.name.begin(), msg_joint_states_.name.end(), msg_joint_states_.position.begin(),
00107 //                  std::inserter( joint_state_map, joint_state_map.end() ),
00108 //                  std::make_pair);
00109   std::vector<double>::const_iterator itPos = msg_joint_states_.position.begin();
00110   for(std::vector<std::string>::const_iterator itName = msg_joint_states_.name.begin();
00111       itName != msg_joint_states_.name.end();
00112       ++itName, ++itPos)
00113   {
00114     joint_state_map[*itName] = *itPos;
00115   }
00116 
00117   // for mimic map
00118   for(MimicMap::iterator i = mimic_.begin(); i != mimic_.end(); i++){
00119     if(joint_state_map.find(i->second->joint_name) != joint_state_map.end()){
00120       double pos = joint_state_map[i->second->joint_name] * i->second->multiplier + i->second->offset;
00121       joint_state_map[i->first] = pos;
00122     }
00123   }
00124 
00125   // reset the transforms we want to use at this time
00126   tf_transforms_.clear();
00127   static const std::string& jt_tf_prefix = "";
00128   setTransforms(joint_state_map, stamp, jt_tf_prefix);
00129   setFixedTransforms(jt_tf_prefix, stamp);
00130 
00135   /*
00136    * can be called via getRobotPosture
00137    * but this would require a proper URDF 
00138    * with a base_link and base_footprint in the base
00139    */
00140   std::vector<float> al_odometry_data = p_motion_.call<std::vector<float> >( "getPosition", "Torso", 1, true );
00141   const ros::Time& odom_stamp = ros::Time::now();
00142   const float& odomX  =  al_odometry_data[0];
00143   const float& odomY  =  al_odometry_data[1];
00144   const float& odomZ  =  al_odometry_data[2];
00145   const float& odomWX =  al_odometry_data[3];
00146   const float& odomWY =  al_odometry_data[4];
00147   const float& odomWZ =  al_odometry_data[5];
00148   //since all odometry is 6DOF we'll need a quaternion created from yaw
00149   tf2::Quaternion tf_quat;
00150   tf_quat.setRPY( odomWX, odomWY, odomWZ );
00151   geometry_msgs::Quaternion odom_quat = tf2::toMsg( tf_quat );
00152 
00153   static geometry_msgs::TransformStamped msg_tf_odom;
00154   msg_tf_odom.header.frame_id = "odom";
00155   msg_tf_odom.child_frame_id = "base_link";
00156   msg_tf_odom.header.stamp = odom_stamp;
00157 
00158   msg_tf_odom.transform.translation.x = odomX;
00159   msg_tf_odom.transform.translation.y = odomY;
00160   msg_tf_odom.transform.translation.z = odomZ;
00161   msg_tf_odom.transform.rotation = odom_quat;
00162 
00163   tf_transforms_.push_back( msg_tf_odom );
00164   tf2_buffer_->setTransform( msg_tf_odom, "naoqiconverter", false);
00165 
00166   if (robot_ == robot::NAO )
00167   {
00168     nao::addBaseFootprint( tf2_buffer_, tf_transforms_, odom_stamp-ros::Duration(0.1) );
00169   }
00170 
00171   // If nobody uses that buffer, do not fill it next time
00172   if (( tf2_buffer_ ) && ( tf2_buffer_.use_count() == 1 ))
00173   {
00174     tf2_buffer_.reset();
00175   }
00176 
00177   for_each( message_actions::MessageAction action, actions )
00178   {
00179     callbacks_[action]( msg_joint_states_, tf_transforms_ );
00180   }
00181 }
00182 
00183 
00184 // Copied from robot state publisher
00185 void JointStateConverter::setTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time, const std::string& tf_prefix)
00186 {
00187   geometry_msgs::TransformStamped tf_transform;
00188   tf_transform.header.stamp = time;
00189 
00190   // loop over all joints
00191   for (std::map<std::string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){
00192     std::map<std::string, robot_state_publisher::SegmentPair>::const_iterator seg = segments_.find(jnt->first);
00193     if (seg != segments_.end()){
00194       seg->second.segment.pose(jnt->second).M.GetQuaternion(tf_transform.transform.rotation.x,
00195                                                             tf_transform.transform.rotation.y,
00196                                                             tf_transform.transform.rotation.z,
00197                                                             tf_transform.transform.rotation.w);
00198       tf_transform.transform.translation.x = seg->second.segment.pose(jnt->second).p.x();
00199       tf_transform.transform.translation.y = seg->second.segment.pose(jnt->second).p.y();
00200       tf_transform.transform.translation.z = seg->second.segment.pose(jnt->second).p.z();
00201 
00202       //tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
00203       //tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
00204       tf_transform.header.frame_id = seg->second.root; // tf2 does not suppport tf_prefixing
00205       tf_transform.child_frame_id = seg->second.tip;
00206 
00207       tf_transforms_.push_back(tf_transform);
00208 
00209       if (tf2_buffer_)
00210           tf2_buffer_->setTransform(tf_transform, "naoqiconverter", false);
00211     }
00212   }
00213   //tf_broadcaster_.sendTransform(tf_transforms);
00214 }
00215 
00216 // Copied from robot state publisher
00217 void JointStateConverter::setFixedTransforms(const std::string& tf_prefix, const ros::Time& time)
00218 {
00219   geometry_msgs::TransformStamped tf_transform;
00220   tf_transform.header.stamp = time/*+ros::Duration(0.5)*/;  // future publish by 0.5 seconds
00221 
00222   // loop over all fixed segments
00223   for (std::map<std::string, robot_state_publisher::SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){
00224     seg->second.segment.pose(0).M.GetQuaternion(tf_transform.transform.rotation.x,
00225                                                 tf_transform.transform.rotation.y,
00226                                                 tf_transform.transform.rotation.z,
00227                                                 tf_transform.transform.rotation.w);
00228     tf_transform.transform.translation.x = seg->second.segment.pose(0).p.x();
00229     tf_transform.transform.translation.y = seg->second.segment.pose(0).p.y();
00230     tf_transform.transform.translation.z = seg->second.segment.pose(0).p.z();
00231 
00232     //tf_transform.header.frame_id = tf::resolve(tf_prefix, seg->second.root);
00233     //tf_transform.child_frame_id = tf::resolve(tf_prefix, seg->second.tip);
00234     tf_transform.header.frame_id = seg->second.root;
00235     tf_transform.child_frame_id = seg->second.tip;
00236 
00237     tf_transforms_.push_back(tf_transform);
00238 
00239     if (tf2_buffer_)
00240       tf2_buffer_->setTransform(tf_transform, "naoqiconverter", true);
00241   }
00242   //tf_broadcaster_.sendTransform(tf_transforms);
00243 }
00244 
00245 void JointStateConverter::addChildren(const KDL::SegmentMap::const_iterator segment)
00246 {
00247   const std::string& root = GetTreeElementSegment(segment->second).getName();
00248 
00249   const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
00250   for (unsigned int i=0; i<children.size(); i++){
00251     const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
00252     robot_state_publisher::SegmentPair s(GetTreeElementSegment(children[i]->second), root, child.getName());
00253     if (child.getJoint().getType() == KDL::Joint::None){
00254       segments_fixed_.insert(std::make_pair(child.getJoint().getName(), s));
00255       ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
00256     }
00257     else{
00258       segments_.insert(std::make_pair(child.getJoint().getName(), s));
00259       ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
00260     }
00261     addChildren(children[i]);
00262   }
00263 }
00264 
00265 } //publisher
00266 } // naoqi


naoqi_driver
Author(s): Karsten Knese
autogenerated on Sun Sep 17 2017 02:57:14