void PlatformCtrlNode::receiveOdo(const sensor_msgs::JointState& js) { mutex.lock(); //odometry msgs nav_msgs::Odometry odom; odom.header.stamp = ros::Time::now(); odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; kin->execForwKin(js, odom, p); topicPub_Odometry.publish(odom); //odometry transform: if(sendTransform) { geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = odom.header.stamp; odom_trans.header.frame_id = odom.header.frame_id; odom_trans.child_frame_id = odom.child_frame_id; odom_trans.transform.translation.x = odom.pose.pose.position.x; odom_trans.transform.translation.y = odom.pose.pose.position.y; odom_trans.transform.translation.z = odom.pose.pose.position.z; odom_trans.transform.rotation = odom.pose.pose.orientation; odom_broadcaster.sendTransform(odom_trans); } mutex.unlock(); }