can I publish my own path and display in Rviz
Hello,
I am trying to display my own custom path in Rviz. I know that I have to use the nav_stack to move the robot. All I want to do is see the path. When I echo
the topic I can see that is publish information but I cannot see it in Rviz. Is the nav_stack the only one who can use the Path
message and display it in Rviz? It should be pretty straight forward.
I am using this to initialize the path
#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
import sending_simple_goals
from geometry_msgs.msg import Pose
def move():
pub = rospy.Publisher('chatter', String, queue_size=10)
my_path = sending_simple_goals.sending_simple_goals(1,.1)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
loc = Pose()
path = []
for ii in xrange(200):
loc.position.x = ii
loc.position.y = ii*2
path.append(loc)
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
pub.publish(hello_str)
my_path.new_path(path)
rate.sleep()
if __name__ == '__main__':
try:
move()
except rospy.ROSInterruptException:
pass
This is the code that is publishing the path
#!/usr/bin/env python
import roslib
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist, PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from std_msgs.msg import String
class sending_simple_goals():
"""docstring fosend_goals_nav"""
def __init__(self, step,break_out_time):
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.path_pub = rospy.Publisher("/aPath",Path,queue_size=100)
#rospy.init_node('sending_simple_goals', anonymous=True)
self.step = step
self.break_out_time = break_out_time
def new_path(self,path):
#check to make sure there is a enough poses to make a path
if len(path) < 2:
rospy.loginfo("NO PATH")
return
#temp varible to hold pose
#posestamped list
pose_list = list()
#path to send the Rviz
my_path = Path()
#my_path.header.stamp = rospy.Time.now()
my_path.header.frame_id = 'odom'
#make the poses into posestamped
for loc in path:
pose = PoseStamped()
pose.pose = loc
# pose.header.frame_id = '/odom'
# pose.header.stamp = rospy.Time.now()
pose_list.append(pose)
my_path.poses.append(pose)
self.path_pub.publish(my_path)
ii = self.step
#loop through all the points in the list
while ii < len(pose_list):
pose_list[ii].header.stamp = rospy.Time.now()
self.move(pose_list[ii])
ii = ii + self.step
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
#set the new goal
pose_list[-1].header.stamp = rospy.Time.now()
goal.target_pose = pose_list[-1]
self.move_base.send_goal(goal)
def move(self,pose):
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
#set the new goal
goal.target_pose = pose
# Send the goal pose to the MoveBaseAction server
self.move_base.send_goal(goal)
#get time
# Allow 1 minute to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(self.break_out_time))
# If we don't get there in time, abort the goal
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
print "time out"
else:
# We made it!
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
print "goal"
Here is a screen ...