Ask Your Question
0

can I publish my own path and display in Rviz

asked 2016-01-12 13:09:18 -0500

ngoldfarb gravatar image

updated 2016-01-13 13:17:39 -0500

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 ... (more)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-01-13 14:49:41 -0500

ngoldfarb gravatar image

It turns out that I was just adding and therefore displaying only 1 point. I had to change the code this part of the code.

 path = list()
for ii in xrange(20):
    loc = Pose()#moved in to for loop
    loc.position.x = ii
    loc.position.y = 2*ii  
    loc.position.z = 0
    path.append(loc)
edit flag offensive delete link more
0

answered 2016-01-12 13:55:59 -0500

Boris_il_forte gravatar image

have you added the Path plugin in the rviz config? add -> path and then you have to select the correct topic (/aPath)

edit flag offensive delete link more

Comments

Yep, and it shows no errors

ngoldfarb gravatar imagengoldfarb ( 2016-01-12 16:43:47 -0500 )edit

my_path.header.frame_id = 'odom' I think should be 'world' (or 'map')

Boris_il_forte gravatar imageBoris_il_forte ( 2016-01-12 17:03:27 -0500 )edit

I tried both and it is still not appearing. I read this which leads me to believe that it is possible and I am just missing something obvious.

ngoldfarb gravatar imagengoldfarb ( 2016-01-12 18:32:03 -0500 )edit

Ok, I'm sure that if you want to show a path in rviz you should only publish the topic. See the (overcomplicated) example here: https://github.com/AIRLab-POLIMI/Simp...

Boris_il_forte gravatar imageBoris_il_forte ( 2016-01-13 01:49:52 -0500 )edit

The only difference that I can see wrt your code is that each trasform has an appropriate stamp and a frame name

Boris_il_forte gravatar imageBoris_il_forte ( 2016-01-13 01:51:06 -0500 )edit

I have tried all combinations of giving stamps and frame names. I can see the messages being received by Rviz.

ngoldfarb gravatar imagengoldfarb ( 2016-01-13 13:14:26 -0500 )edit

uhm, I don't have any clues then. I'm sure that all you have to do is tu publish the message and add a path entry in rviz and setup the correct topic... Because I've recently worked on the code for doing so (the one that I've linked to you). I'm sorry I couldn't help you...

Boris_il_forte gravatar imageBoris_il_forte ( 2016-01-13 13:41:06 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-01-12 13:09:18 -0500

Seen: 3,251 times

Last updated: Jan 13 '16