Answered step by step
Verified Expert Solution
Link Copied!

Question

1 Approved Answer

Please help with python ROS code. How would you implement what the comments under the timer function are asking for? I am completely lost and

Please help with python ROS code. How would you implement what the comments under the "timer" function are asking for? I am completely lost and have no idea what I am doing:
#!/usr/bin/env python3
from grid import grid2D
import rospy
from nav_msgs.msg import Odometry, OccupancyGrid, Path
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import PoseStamped
import numpy as np
import pdb
from copy import copy
class planner():
def __init__(self):
self.goal=None
self.current_xyT=np.zeros((3),dtype=float)
# ROS Subscriber functions
self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_cb)
self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb)
self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb)
self.timer_sub = rospy.Timer(rospy.Duration(5), self.timer)
# ROS publishers
self.path_pub = rospy.Publisher("/path", Path, queue_size=5)
self.target_pub = rospy.Publisher("/target_xy", PoseStamped, queue_size=5)
# Map Cb - build the grid
def map_cb(self, map_msg: OccupancyGrid):
print("Map Received")
self.map=grid2D(map_msg)
self.map_sub.unregister()
# Retrieve the latest robot position
def odom_cb(self, odom_msg:Odometry):
self.current_xyT[0]=odom_msg.pose.pose.position.x
self.current_xyT[1]=odom_msg.pose.pose.position.y
quat=[odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w]
rpy=euler_from_quaternion(quat)
self.current_xyT[2]=rpy[2]
def goal_cb(self, target: PoseStamped):
# Build a plan to reach the target from the current robot position
self.goal=np.array([target.pose.position.x, target.pose.position.y])
def publish_path(self, path_arr: np.ndarray):
# path_arr should be of format [[X1, Y1],[X2, Y2],...,[Xn, Yn]]
pth=Path()
pth.header.frame_id="/map"
pth.header.stamp=rospy.Time()
for idx in range(path_arr.shape[0]):
PS=PoseStamped()
PS.header=pth.header
PS.pose.position.x=path_arr[idx,0]
PS.pose.position.y=path_arr[idx,1]
PS.pose.orientation.w=1.0
pth.poses.append(PS)
self.path_pub.publish(pth)
def publish_target(self, X, Y):
ps=PoseStamped()
ps.header.frame_id="/map"
ps.header.stamp=rospy.Time()
ps.pose.position.x=X
ps.pose.position.y=Y
self.target_pub.publish(ps)
def timer(self, tmr):
# You need to build a path consisting of a series of waypoints
# in XY space and send them one at a time to the behavioral
# controller
# A few things to consider:
# 1) What space can the robot actually reach? You have a map, but it
# represents space thought to contain obstacles
# 2) How finely do you sample the path? Do you need to send a new waypoint
# every 5mm? That's the default map grid resolution
# 3) We will add new waypoints via RViz interface so you do need to rebuild.
# How often you rebuild is up to you.
# Sample - Create a path (REPLACE)
path=np.array([[1,0],[1,2],[-1,2],[-4,3],[-6,1]])
# Sample - Publish path and a target for the behavior-based controller
self.publish_path(path)
self.publish_target(path[0,0],path[0,1])
if __name__=='__main__':
rospy.init_node("planner")
pl=planner()
rospy.spin()

Step by Step Solution

There are 3 Steps involved in it

Step: 1

blur-text-image

Get Instant Access to Expert-Tailored Solutions

See step-by-step solutions with expert insights and AI powered tools for academic success

Step: 2

blur-text-image_2

Step: 3

blur-text-image_3

Ace Your Homework with AI

Get the answers you need in no time with our AI-driven, step-by-step assistance

Get Started

Recommended Textbook for

Beginning VB 2008 Databases

Authors: Vidya Vrat Agarwal, James Huddleston

1st Edition

1590599470, 978-1590599471

More Books

Students also viewed these Databases questions

Question

What is the basis for Security Concerns in Cloud Computing?

Answered: 1 week ago

Question

Describe the three main Cloud Computing Environments.

Answered: 1 week ago