# 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,
