Rviz Marker Array Does Not Display When Using SMACH

edit

Hello,

This is my first time using ROS Answers so bear with me. I am new to ROS and python also. I am trying to write a node that reads in waypoints of a map from a .yaml file, displays these waypoints in rviz using rviz markers, and then a simulated turtlebot patrols these waypoints using smach. Here is an example .yaml file.

- {filename: 'one', position: {x: 2.1, y: 2.2, z: 0.0}, quaternion: {r1: 0.0, r2: 0.0, r3: 0.0, r4: 1.0}}
- {filename: 'two', position: {x: 6.5, y: 4.43, z: 0.0}, quaternion: {r1: 0.0, r2: 0.0, r3: -0.984047240305, r4: 0.177907360295}}
- {filename: 'two', position: {x: 6.5, y: 4.43, z: 0.0}, quaternion: {r1: 0.0, r2: 0.0, r3: -0.984047240305, r4: 0.177907360295}}

And here is my node.

#!/usr/bin/env python

# Used example code from Programming Robots with ROS text.

import rospy
import yaml

from smach import State, StateMachine
from smach_ros import SimpleActionState
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Twist

def mark_waypoints(waypoints):
    markerArray = MarkerArray()
    id_count = 1

    for pose in waypoints:
        markerText = Marker()
        markerText.header.frame_id = 'map'
        markerText.header.stamp = rospy.Time()
        markerText.ns = 'waypointText'
        markerText.id = id_count
        markerText.type = markerText.TEXT_VIEW_FACING
        markerText.action = markerText.ADD
        markerText.pose.position.x = pose['position']['x']
        markerText.pose.position.y = pose['position']['y']
        markerText.pose.position.z = pose['position']['z']
        markerText.pose.orientation.x = 0.0
        markerText.pose.orientation.y = 0.0
        markerText.pose.orientation.z = 0.0
        markerText.pose.orientation.w = 1.0
        markerText.scale.x = 0.75
        markerText.scale.y = 0.75
        markerText.scale.z = 0.75
        markerText.color.a = 1.0
        markerText.color.r = 0.0
        markerText.color.g = 0.0
        markerText.color.b = 1.0
        markerText.text = str(id_count)

        markerArray.markers.append(markerText)
        id_count += 1

    return markerArray    

if __name__ == '__main__':
    rospy.init_node('patrol3')

    marker_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1)

    with open('/home/ros/catkin_ws/src/exercise2/scripts/yaml/waypoints.yaml', 'r') as stream:
        waypoints = yaml.load(stream)

    while not rospy.is_shutdown():
    #Markers showed up when the patrol aborted?
        markerArray = mark_waypoints(waypoints)
        marker_pub.publish(markerArray)

        patrol = StateMachine(['succeeded','aborted','preempted'])
        with patrol:
            for i,w in enumerate(waypoints):
                goal_pose = MoveBaseGoal()
                goal_pose.target_pose.header.frame_id = 'map'
                goal_pose.target_pose.pose.position.x = w['position']['x']
                goal_pose.target_pose.pose.position.y = w['position']['y']
                goal_pose.target_pose.pose.position.z = 0.0
                goal_pose.target_pose.pose.orientation.x = w['quaternion']['r1']
                goal_pose.target_pose.pose.orientation.y = w['quaternion']['r2']
                goal_pose.target_pose.pose.orientation.z = w['quaternion']['r3']
                goal_pose.target_pose.pose.orientation.w = w['quaternion']['r4']

        patrol.execute()

I am using the turtlebot_stage for my simulations. So far my robot appears to continuously patrol the waypoints but the waypoints are not marked. I am not sure why the markerArray is not being displayed in rviz. I also noticed that when the state machine was aborted all of the markers popped up in rviz.

Any advice/guidance is greatly appreciated.