# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. from sensor_msgs.msg import PointCloud2 import PyKDL import rospy import tf2_ros from sensor_msgs.point_cloud2 import read_cloud def to_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg) def from_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame( PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w),
return res tf2_ros.TransformRegistration().add(PyKDL.Vector, do_transform_vector) def to_msg_vector(vector): msg = PointStamped() msg.header = vector.header msg.point.x = vector[0] msg.point.y = vector[1] msg.point.z = vector[2] return msg tf2_ros.ConvertRegistration().add_to_msg(PyKDL.Vector, to_msg_vector) def from_msg_vector(msg): vector = PyKDL.Vector(msg.point.x, msg.point.y, msg.point.z) return tf2_ros.Stamped(vector, msg.header.stamp, msg.header.frame_id) tf2_ros.ConvertRegistration().add_from_msg(PyKDL.Vector, from_msg_vector) def convert_vector(vector): return tf2_ros.Stamped(PyKDL.Vector(vector), vector.header.stamp, vector.header.frame_id)
# POSSIBILITY OF SUCH DAMAGE. # author: Wim Meeussen from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped import PyKDL import rospy import tf2_ros import copy def to_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_to_msg(Vector3Stamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PoseStamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(PointStamped, to_msg_msg) def from_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_from_msg(Vector3Stamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PoseStamped, from_msg_msg) tf2_ros.ConvertRegistration().add_from_msg(PointStamped, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame(
from tf2_ros import Stamped from sensor_msgs.msg import Imu from nav_msgs.msg import Odometry import PyKDL import copy from geometry_msgs.msg import TransformStamped, PointStamped, Vector3Stamped, PoseStamped, WrenchStamped from geometry_msgs.msg import TwistStamped from geometry_msgs.msg import PoseStamped, Vector3Stamped, PointStamped, WrenchStamped, QuaternionStamped def to_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_to_msg(QuaternionStamped, to_msg_msg) tf2_ros.ConvertRegistration().add_to_msg(TwistStamped, to_msg_msg) def from_msg_msg(msg): return msg tf2_ros.ConvertRegistration().add_from_msg(QuaternionStamped, from_msg_msg) def transform_to_kdl(t): return PyKDL.Frame( PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z,