27 #include <boost/foreach.hpp>
28 #define for_each BOOST_FOREACH
45 p_motion_(
session->service(
"ALMotion").value()),
46 p_memory_(
session->service(
"ALMemory").value()),
47 tf2_buffer_(tf2_buffer)
61 std::cout <<
"error in loading robot description" << std::endl;
73 for(std::map< std::string, urdf::JointSharedPtr >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++){
75 mimic_.insert(make_pair(i->first, i->second->mimic));
90 std::vector<double> al_joint_angles =
p_motion_.call<std::vector<double> >(
"getAngles",
"Body", true );
91 std::vector<double> al_joint_velocities;
92 std::vector<double> al_joint_torques;
102 msg_joint_states_.position = std::vector<double>( al_joint_angles.begin(), al_joint_angles.end() );
108 std::map< std::string, double > joint_state_map;
114 for(std::vector<std::string>::const_iterator itName =
msg_joint_states_.name.begin();
118 joint_state_map[*itName] = *itPos;
121 al_joint_velocities.push_back(
p_memory_.call<
double>(
123 "Motion/Velocity/Sensor/" + (*itName)));
125 al_joint_torques.push_back(
p_memory_.call<
double>(
127 "Motion/Torque/Sensor/" + (*itName)));
129 }
catch (qi::FutureUserException e) {
131 al_joint_velocities.push_back(std::numeric_limits<double>::quiet_NaN());
132 al_joint_torques.push_back(std::numeric_limits<double>::quiet_NaN());
140 for(MimicMap::iterator i =
mimic_.begin(); i !=
mimic_.end(); i++){
141 if(joint_state_map.find(i->second->joint_name) != joint_state_map.end()){
142 double pos = joint_state_map[i->second->joint_name] * i->second->multiplier + i->second->offset;
143 joint_state_map[i->first] = pos;
149 static const std::string& jt_tf_prefix =
"";
162 std::vector<float> al_odometry_data =
p_motion_.call<std::vector<float> >(
"getPosition",
"Torso", 1, true );
164 const float& odomX = al_odometry_data[0];
165 const float& odomY = al_odometry_data[1];
166 const float& odomZ = al_odometry_data[2];
167 const float& odomWX = al_odometry_data[3];
168 const float& odomWY = al_odometry_data[4];
169 const float& odomWZ = al_odometry_data[5];
172 tf_quat.
setRPY( odomWX, odomWY, odomWZ );
173 geometry_msgs::Quaternion odom_quat =
tf2::toMsg( tf_quat );
175 static geometry_msgs::TransformStamped msg_tf_odom;
176 msg_tf_odom.header.frame_id =
"odom";
177 msg_tf_odom.child_frame_id =
"base_link";
178 msg_tf_odom.header.stamp = odom_stamp;
180 msg_tf_odom.transform.translation.x = odomX;
181 msg_tf_odom.transform.translation.y = odomY;
182 msg_tf_odom.transform.translation.z = odomZ;
183 msg_tf_odom.transform.rotation = odom_quat;
186 tf2_buffer_->setTransform( msg_tf_odom,
"naoqiconverter",
false);
209 geometry_msgs::TransformStamped tf_transform;
210 tf_transform.header.stamp = time;
213 for (std::map<std::string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){
214 std::map<std::string, robot_state_publisher::SegmentPair>::const_iterator seg =
segments_.find(jnt->first);
216 seg->second.segment.pose(jnt->second).M.GetQuaternion(tf_transform.transform.rotation.x,
217 tf_transform.transform.rotation.y,
218 tf_transform.transform.rotation.z,
219 tf_transform.transform.rotation.w);
220 tf_transform.transform.translation.x = seg->second.segment.pose(jnt->second).p.x();
221 tf_transform.transform.translation.y = seg->second.segment.pose(jnt->second).p.y();
222 tf_transform.transform.translation.z = seg->second.segment.pose(jnt->second).p.z();
226 tf_transform.header.frame_id = seg->second.root;
227 tf_transform.child_frame_id = seg->second.tip;
232 tf2_buffer_->setTransform(tf_transform,
"naoqiconverter",
false);
241 geometry_msgs::TransformStamped tf_transform;
242 tf_transform.header.stamp = time;
246 seg->second.segment.pose(0).M.GetQuaternion(tf_transform.transform.rotation.x,
247 tf_transform.transform.rotation.y,
248 tf_transform.transform.rotation.z,
249 tf_transform.transform.rotation.w);
250 tf_transform.transform.translation.x = seg->second.segment.pose(0).p.x();
251 tf_transform.transform.translation.y = seg->second.segment.pose(0).p.y();
252 tf_transform.transform.translation.z = seg->second.segment.pose(0).p.z();
256 tf_transform.header.frame_id = seg->second.root;
257 tf_transform.child_frame_id = seg->second.tip;
262 tf2_buffer_->setTransform(tf_transform,
"naoqiconverter",
true);
269 const std::string& root = GetTreeElementSegment(segment->second).getName();
271 const std::vector<KDL::SegmentMap::const_iterator>& children = GetTreeElementChildren(segment->second);
272 for (
unsigned int i=0; i<children.size(); i++){
273 const KDL::Segment& child = GetTreeElementSegment(children[i]->second);
277 ROS_DEBUG(
"Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
280 segments_.insert(std::make_pair(child.getJoint().getName(),
s));
281 ROS_DEBUG(
"Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());