Rviz Marker Array Does Not Display When Using SMACH
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.
Just a comment: you seem to be re-creating your SMACH statemachine every iteration of your while loop. While technically perfectly valid, I don't think that's what you want to do. Create the SM once, add your states, then let it run.