Browse Source

Included additional links.

main
Alan G. Sanchez 2 years ago
parent
commit
4c901de0ed
2 changed files with 13 additions and 13 deletions
  1. +9
    -9
      example_2.md
  2. +4
    -4
      src/scan_filter.py

+ 9
- 9
example_2.md View File

@ -44,7 +44,7 @@ First, open a terminal and run the stretch driver launch file.
roslaunch stretch_core stretch_driver.launch
```
Then in a new terminal run the rplidar launch file from `stretch_core`.
Then in a new terminal run the `rplidar.launch` file from `stretch_core`.
```bash
# Terminal 2
roslaunch stretch_core rplidar.launch
@ -82,7 +82,7 @@ from sensor_msgs.msg import LaserScan
class ScanFilter:
"""
A class that implements a LaserScan filter that removes all of the points
that are not infront of the robot.
that are not in front of the robot.
"""
def __init__(self):
self.width = 1.0
@ -93,11 +93,11 @@ class ScanFilter:
def callback(self,msg):
"""
Callback function to deal with incoming laserscan messages.
Callback function to deal with incoming LaserScan messages.
:param self: The self reference.
:param msg: The subscribed laserscan message.
:param msg: The subscribed LaserScan message.
:publishes msg: updated laserscan message.
:publishes msg: updated LaserScan message.
"""
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
points = [r * sin(theta) if (theta < -2.5 or theta > 2.5) else inf for r,theta in zip(msg.ranges, angles)]
@ -127,7 +127,7 @@ from numpy import linspace, inf
from math import sin
from sensor_msgs.msg import LaserScan
```
You need to import rospy if you are writing a ROS Node. There are functions from numpy and math that are required within this code, thus why linspace, inf, and sin are imported. The `sensor_msgs.msg` import is so that we can subscribe and publish LaserScan messages.
You need to import `rospy` if you are writing a ROS [Node](http://wiki.ros.org/Nodes). There are functions from `numpy` and `math` that are required within this code, thus why linspace, inf, and sin are imported. The `sensor_msgs.msg` import is so that we can subscribe and publish `LaserScan` messages.
```python
self.width = 1
@ -138,12 +138,12 @@ We're going to assume that the robot is pointing up the x-axis, so that any poin
```python
self.sub = rospy.Subscriber('/scan', LaserScan, self.callback)
```
Set up a subscriber. We're going to subscribe to the topic *scan*, looking for LaserScan messages. When a message comes in, ROS is going to pass it to the function "callback" automatically.
Set up a subscriber. We're going to subscribe to the topic *scan*, looking for `LaserScan` messages. When a message comes in, ROS is going to pass it to the function "callback" automatically.
```python
self.pub = rospy.Publisher('filtered_scan', LaserScan, queue_size=10)
```
`pub = rospy.Publisher("filtered_scan", LaserScan, queue_size=10)` declares that your node is publishing to the *filtered_scan* topic using the message type LaserScan. This lets the master tell any nodes listening on *filtered_scan* that we are going to publish data on that topic.
`pub = rospy.Publisher("filtered_scan", LaserScan, queue_size=10)` declares that your node is publishing to the *filtered_scan* topic using the message type `LaserScan`. This lets the master tell any nodes listening on *filtered_scan* that we are going to publish data on that topic.
```python
angles = linspace(msg.angle_min, msg.angle_max, len(msg.ranges))
@ -171,7 +171,7 @@ Substitute in the new ranges in the original message, and republish it.
rospy.init_node('scan_filter')
ScanFilter()
```
The next line, `rospy.init_node(NAME, ...)`, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. In this case, your node will take on the name talker. NOTE: the name must be a base name, i.e. it cannot contain any slashes "/".
The next line, `rospy.init_node(NAME, ...)`, is very important as it tells rospy the name of your node -- until rospy has this information, it cannot start communicating with the ROS Master. **NOTE:** the name must be a base name, i.e. it cannot contain any slashes "/".
Instantiate the class with `ScanFilter()`

+ 4
- 4
src/scan_filter.py View File

@ -11,7 +11,7 @@ from sensor_msgs.msg import LaserScan
class ScanFilter:
"""
A class that implements a LaserScan filter that removes all of the points.
that are not infront of the robot.
that are not in front of the robot.
"""
def __init__(self):
# We're going to assume that the robot is pointing up the x-axis, so that
@ -34,11 +34,11 @@ class ScanFilter:
def callback(self,msg):
"""
Callback function to deal with incoming laserscan messages.
Callback function to deal with incoming LaserScan messages.
:param self: The self reference.
:param msg: The subscribed laserscan message.
:param msg: The subscribed LaserScan message.
:publishes msg: updated laserscan message.
:publishes msg: updated LaserScan message.
"""
# Figure out the angles of the scan. We're going to do this each time,
# in case we're subscribing to more than one laser, with different numbers of beams

Loading…
Cancel
Save