visualization dual quaternions with RViz

dq_viz.py

#!/usr/bin/env python

import rospy
import copy
import numpy as np

from interactive_markers.interactive_marker_server import *
from visualization_msgs.msg import *
from dual_quaternions_ros import DualQuaternion
from geometry_msgs.msg import Vector3, Point, Pose
from std_msgs.msg import ColorRGBA

from visualization_msgs.msg import Marker, MarkerArray


class DQVizTool(object):
    def __init__(self):
        self.marker_pub = rospy.Publisher("visualization_marker_array", MarkerArray, queue_size=3)
        self.im_server = InteractiveMarkerServer("pose_tweaker")
        self.uid = 1

        # make the parent marker
        int_marker = InteractiveMarker()
        int_marker.header.frame_id = "world"
        int_marker.scale = 0.7
        int_marker.pose.orientation.w = 1.
        int_marker.name = "pose_tweaker"

        # make the visualization of the object getting transformed
        box_marker = self.make_transformation_object(pose=DualQuaternion.identity().ros_pose(),
                                                     color=ColorRGBA(0, 0, 1, 1))

        # make the control marker
        control = InteractiveMarkerControl()
        control.always_visible = True
        control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
        control.orientation.w = 1.
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.always_visible = True
        control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
        control.orientation.w = 0.707
        control.orientation.z = 0.707
        int_marker.controls.append(control)

        control_v = InteractiveMarkerControl()
        control_v.markers.append(box_marker)
        control_v.always_visible = True
        int_marker.controls.append(control_v)

        self.im_server.insert(int_marker, self.process_feedback)
        self.im_server.applyChanges()

    def process_feedback(self, feedback):
        final_pose_dq = DualQuaternion.from_ros_pose(feedback.pose)
        final_box = self.make_transformation_object(feedback.pose)

        # screw params
        l, m, theta, d = final_pose_dq.screw()
        if m[0] == np.inf:
            p_ = np.array([0, 0, 0])
        else:
            # closest point on the axis
            p_ = np.cross(l, m)
        tail_point = Point(*p_)
        p_head = p_ + d * l
        head_point = Point(*p_head)

        axis_marker = Marker()
        axis_marker.id = 0
        axis_marker.type = Marker.ARROW
        axis_marker.header.stamp = rospy.Time.now()
        axis_marker.header.frame_id = "world"
        axis_marker.points = [tail_point, head_point]
        axis_marker.scale = Vector3(0.05, 0.1, 0.2)
        axis_marker.color = ColorRGBA(1.0, 0., 0., 0.9)

        # generate intermediate markers
        intermediate_boxes = []
        self.uid = 10
        nb_boxes = 70
        taus = np.linspace(0.0, 1.0, num=nb_boxes)
        for tau in taus:
            intermediate_dq = final_pose_dq.sclerp(DualQuaternion.identity(), final_pose_dq, tau)
            intermediate_marker = self.make_transformation_object(intermediate_dq.ros_pose(),
                                                                  color=ColorRGBA(0., 1., 0., np.log(tau+1)/5))
            intermediate_boxes.append(intermediate_marker)

        marray = MarkerArray()
        marray.markers = [axis_marker, final_box] + intermediate_boxes

        self.marker_pub.publish(marray)

    def make_transformation_object(self, pose, color=ColorRGBA(0., 1., 0., 0.1), scale=Vector3(0.2, 0.4, 0.5)):
        obj_marker = Marker()
        obj_marker.id = self.uid
        self.uid += 1
        obj_marker.type = Marker.CUBE
        obj_marker.pose = pose
        obj_marker.header.stamp = rospy.Time.now()
        obj_marker.header.frame_id = "world"
        obj_marker.color = color
        obj_marker.scale = scale

        return obj_marker


if __name__ == '__main__':
    rospy.init_node("dual_quaternion_viz_node", log_level=rospy.DEBUG)
    viz_tool = DQVizTool()
    rospy.spin()