Answered step by step
Verified Expert Solution
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:
#usrbinenv python
from grid import gridD
import rospy
from navmsgsmsg import Odometry, OccupancyGrid, Path
from tftransformations import eulerfromquaternion
from geometrymsgsmsg import PoseStamped
import numpy as np
import pdb
from copy import copy
class planner:
def initself:
self.goalNone
self.currentxyTnpzerosdtypefloat
# ROS Subscriber functions
self.odomsub rospy.Subscriberodom Odometry, self.odomcb
self.mapsub rospy.Subscribermap OccupancyGrid, self.mapcb
self.goalsub rospy.Subscribermovebasesimplegoal PoseStamped, self.goalcb
self.timersub rospy.TimerrospyDuration self.timer
# ROS publishers
self.pathpub rospy.Publisherpath Path, queuesize
self.targetpub rospy.Publishertargetxy PoseStamped, queuesize
# Map Cb build the grid
def mapcbself mapmsg: OccupancyGrid:
printMap Received"
self.mapgridDmapmsg
self.mapsub.unregister
# Retrieve the latest robot position
def odomcbself odommsg:Odometry:
self.currentxyTodommsgpose.pose.position.x
self.currentxyTodommsgpose.pose.position.y
quatodommsgpose.pose.orientation.x
odommsgpose.pose.orientation.y
odommsgpose.pose.orientation.z
odommsgpose.pose.orientation.w
rpyeulerfromquaternionquat
self.currentxyTrpy
def goalcbself target: PoseStamped:
# Build a plan to reach the target from the current robot position
self.goalnparraytargetpose.position.x target.pose.position.y
def publishpathself patharr: npndarray:
# patharr should be of format X YX YXn Yn
pthPath
pthheader.frameidmap
pthheader.stamprospy.Time
for idx in rangepatharr.shape:
PSPoseStamped
PSheaderpthheader
PSpose.position.xpatharridx
PSpose.position.ypatharridx
PSpose.orientation.w
pthposes.appendPS
self.pathpub.publishpth
def publishtargetself X Y:
psPoseStamped
psheader.frameidmap
psheader.stamprospy.Time
pspose.position.xX
pspose.position.yY
self.targetpub.publishps
def timerself 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:
# What space can the robot actually reach? You have a map, but it
# represents space thought to contain obstacles
# How finely do you sample the path? Do you need to send a new waypoint
# every mm That's the default map grid resolution
# 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
pathnparray
# Sample Publish path and a target for the behaviorbased controller
self.publishpathpath
self.publishtargetpathpath
if namemain:
rospy.initnodeplanner
plplanner
rospy.spin
Step by Step Solution
There are 3 Steps involved in it
Step: 1
Get Instant Access to Expert-Tailored Solutions
See step-by-step solutions with expert insights and AI powered tools for academic success
Step: 2
Step: 3
Ace Your Homework with AI
Get the answers you need in no time with our AI-driven, step-by-step assistance
Get Started