0
$\begingroup$

Rosanswers logo

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 shot of Rviz

image description


Originally posted by ngoldfarb on ROS Answers with karma: 127 on 2016-01-12

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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)

Originally posted by ngoldfarb with karma: 127 on 2016-01-13

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.