Script to publish ROS2 data

PathPublisher.py

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Path
from math import sin, cos, pi
from squaternion import Quaternion

class PathPublisher(Node):

    def __init__(self):
        super().__init__('path_msg_TEST')
        self.publisher_ = self.create_publisher(Path, '/path', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        self.radius = 0.5
        self.increment = 0.5

    def timer_callback(self):
        msg = Path()
        msg.header.frame_id = "world"

        for k in range(19):
            pose = PoseStamped()
            pose.pose.position.x = self.radius * cos(k * 5 * (pi/180))
            pose.pose.position.y = self.radius * sin(k * 5 * (pi/180)) - self.radius
            pose.pose.position.z = 0.0

            quat = Quaternion.from_euler(0.0, 0.0, k*5, degrees=True)

            pose.pose.orientation.x = quat[1]
            pose.pose.orientation.y = quat[2]
            pose.pose.orientation.z = quat[3]
            pose.pose.orientation.w = quat[0]

            msg.poses.append(pose)
        
        self.radius += 0.5
        if self.radius == 5:
            self.radius = 0.5            

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing Path')


def main(args=None):
    rclpy.init(args=args)

    viz_publisher = PathPublisher()

    rclpy.spin(viz_publisher)

    viz_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

PointPublisher.py

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped


class PointPublisher(Node):

    def __init__(self):
        super().__init__('point_msg_TEST')
        self.publisher_ = self.create_publisher(PointStamped, '/point', 10)
        timer_period = 1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0.0

    def timer_callback(self):
        msg = PointStamped()
        msg.header.frame_id = "world"

        msg.point.x = self.i % 10
        msg.point.y = self.i % 10
        msg.point.z = self.i % 10

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing point')
        self.i += 1


def main(args=None):
    rclpy.init(args=args)

    viz_publisher = PointPublisher()

    rclpy.spin(viz_publisher)

    viz_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

PolygonPublisher.py

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PolygonStamped, Point32
from math import sin, cos, pi


class PolygonPublisher(Node):

    def __init__(self):
        super().__init__('polygon_msg_TEST')
        self.publisher_ = self.create_publisher(PolygonStamped, '/polygon', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0.0

    def timer_callback(self):
        msg = PolygonStamped()
        msg.header.frame_id = "world"

        for deg in range(0, 360, 5):
            point = Point32()
            point.x = cos(deg * (pi/180))
            point.y = sin(deg * (pi/180))
            point.z = sin((self.i + deg) * (pi/180))

            if deg % 2 == 1:
                point.x *= 1.5
                point.y *= 1.5

            msg.polygon.points.append(point)

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing polygon')
        self.i += 5


def main(args=None):
    rclpy.init(args=args)

    viz_publisher = PolygonPublisher()

    rclpy.spin(viz_publisher)

    viz_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

PoseArrayPublisher.py

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose, PoseArray
from math import sin, cos, pi
from squaternion import Quaternion

class PoseArrayPublisher(Node):

    def __init__(self):
        super().__init__('pose_array_msg_TEST')
        self.publisher_ = self.create_publisher(PoseArray, '/pose_array', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        self.deg = 0
        self.poses = []
        self.update = 1
        self.max = 30

    def timer_callback(self):
        pose = Pose()

        pose.position.x = 2 * cos(self.deg * (pi/180))
        pose.position.y = 2 * sin(self.deg * (pi/180))
        pose.position.z = 0.0
        x = 2 * cos(self.deg * (pi/180))
        y = 2 * sin(self.deg * (pi/180))
    
        quat = Quaternion.from_euler(0.0, 0.0, self.deg, degrees=True)

        pose.orientation.x = quat[1]
        pose.orientation.y = quat[2]
        pose.orientation.z = quat[3]
        pose.orientation.w = quat[0]

        self.poses.append(pose)

        if(len(self.poses) == self.max):
            self.poses.pop(0)

        msg = PoseArray()
        msg.header.frame_id = "world"
        msg.poses = self.poses[-self.i:]

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing poses')

        self.i += self.update
        self.update = 1 if self.i == 1 else (-1 if self.i == self.max else self.update)
        self.deg += 5


def main(args=None):
    rclpy.init(args=args)

    viz_publisher = PoseArrayPublisher()

    rclpy.spin(viz_publisher)

    viz_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

PosePublisher.py

#!/usr/bin/python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from math import sin, cos, pi
from squaternion import Quaternion

class PosePublisher(Node):

    def __init__(self):
        super().__init__('pose_msg_TEST')
        self.publisher_ = self.create_publisher(PoseStamped, '/pose', 10)
        timer_period = 0.1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
        self.deg = 0

    def timer_callback(self):
        msg = PoseStamped()
        msg.header.frame_id = "world"

        msg.pose.position.x = 2 * cos(self.deg * (pi/180))
        msg.pose.position.y = 2 * sin(self.deg * (pi/180))
        msg.pose.position.z = abs(sin(self.i))
        x = 2 * cos(self.deg * (pi/180))
        y = 2 * sin(self.deg * (pi/180))
    
        quat = Quaternion.from_euler(0.0, 0.0, self.deg, degrees=True)

        msg.pose.orientation.x = quat[1]
        msg.pose.orientation.y = quat[2]
        msg.pose.orientation.z = quat[3]
        msg.pose.orientation.w = quat[0]

        self.publisher_.publish(msg)
        self.get_logger().info('Publishing pose')
        self.i += 1
        self.deg += 5


def main(args=None):
    rclpy.init(args=args)

    viz_publisher = PosePublisher()

    rclpy.spin(viz_publisher)

    viz_publisher.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()