From 9d37b0fcd3ce1b89e0cccffcd367eeb00c827dcd Mon Sep 17 00:00:00 2001 From: Amesh Date: Fri, 1 Oct 2021 21:13:06 +1000 Subject: [PATCH] Amesh's Induction Task --- src/cloud_point2.py | 204 +++++++++++++++++++++++++++++++++++++++ src/obstacle_detector.py | 141 +++++++++++++++++++++++++++ 2 files changed, 345 insertions(+) create mode 100644 src/cloud_point2.py create mode 100755 src/obstacle_detector.py diff --git a/src/cloud_point2.py b/src/cloud_point2.py new file mode 100644 index 0000000..8c28d5a --- /dev/null +++ b/src/cloud_point2.py @@ -0,0 +1,204 @@ + +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +""" +Serialization of sensor_msgs.PointCloud2 messages. +Author: Tim Field +ROS2 port by Sebastian Grans (2020) +""" + +import sys + +from collections import namedtuple +import ctypes +import math +import struct + +from sensor_msgs.msg import PointCloud2, PointField + +_DATATYPES = {} +_DATATYPES[PointField.INT8] = ('b', 1) +_DATATYPES[PointField.UINT8] = ('B', 1) +_DATATYPES[PointField.INT16] = ('h', 2) +_DATATYPES[PointField.UINT16] = ('H', 2) +_DATATYPES[PointField.INT32] = ('i', 4) +_DATATYPES[PointField.UINT32] = ('I', 4) +_DATATYPES[PointField.FLOAT32] = ('f', 4) +_DATATYPES[PointField.FLOAT64] = ('d', 8) + +def read_points(cloud, field_names=None, skip_nans=False, uvs=[]): + """ + Read points from a L{sensor_msgs.PointCloud2} message. + @param cloud: The point cloud to read from. + @type cloud: L{sensor_msgs.PointCloud2} + @param field_names: The names of fields to read. If None, read all fields. [default: None] + @type field_names: iterable + @param skip_nans: If True, then don't return any point with a NaN value. + @type skip_nans: bool [default: False] + @param uvs: If specified, then only return the points at the given coordinates. [default: empty list] + @type uvs: iterable + @return: Generator which yields a list of values for each point. + @rtype: generator + """ + assert isinstance(cloud, PointCloud2), 'cloud is not a sensor_msgs.msg.PointCloud2' + fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names) + width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, \ + cloud.point_step, cloud.row_step, \ + cloud.data, math.isnan + unpack_from = struct.Struct(fmt).unpack_from + + if skip_nans: + if uvs: + for u, v in uvs: + p = unpack_from(data, (row_step * v) + (point_step * u)) + has_nan = False + for pv in p: + if isnan(pv): + has_nan = True + break + if not has_nan: + yield p + else: + for v in range(height): + offset = row_step * v + for u in range(width): + p = unpack_from(data, offset) + has_nan = False + for pv in p: + if isnan(pv): + has_nan = True + break + if not has_nan: + yield p + offset += point_step + else: + if uvs: + for u, v in uvs: + yield unpack_from(data, (row_step * v) + (point_step * u)) + else: + for v in range(height): + offset = row_step * v + for u in range(width): + yield unpack_from(data, offset) + offset += point_step + +def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): + """ + Read points from a L{sensor_msgs.PointCloud2} message. + + This function returns a list of namedtuples. It operates on top of the read_points method. For more efficient access use read_points directly. + + @param cloud: The point cloud to read from. + @type cloud: L{sensor_msgs.PointCloud2} + @param field_names: The names of fields to read. If None, read all fields. [default: None] + @type field_names: iterable + @param skip_nans: If True, then don't return any point with a NaN value. + @type skip_nans: bool [default: False] + @param uvs: If specified, then only return the points at the given coordinates. [default: empty list] + @type uvs: iterable + @return: List of namedtuples containing the values for each point + @rtype: list + """ + assert isinstance(cloud, PointCloud2), 'cloud is not a sensor_msgs.msg.PointCloud2' + + if field_names is None: + field_names = [f.name for f in cloud.fields] + + Point = namedtuple("Point", field_names) + + return [Point._make(l) for l in read_points(cloud, field_names, skip_nans, uvs)] + +def create_cloud(header, fields, points): + """ + Create a L{sensor_msgs.msg.PointCloud2} message. + @param header: The point cloud header. + @type header: L{std_msgs.msg.Header} + @param fields: The point cloud fields. + @type fields: iterable of L{sensor_msgs.msg.PointField} + @param points: The point cloud points. + @type points: list of iterables, i.e. one iterable for each point, with the + elements of each iterable being the values of the fields for + that point (in the same order as the fields parameter) + @return: The point cloud. + @rtype: L{sensor_msgs.msg.PointCloud2} + """ + cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) + + buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) + + point_step, pack_into = cloud_struct.size, cloud_struct.pack_into + offset = 0 + for p in points: + pack_into(buff, offset, *p) + offset += point_step + + return PointCloud2(header=header, + height=1, + width=len(points), + is_dense=False, + is_bigendian=False, + fields=fields, + point_step=cloud_struct.size, + row_step=cloud_struct.size * len(points), + data=buff.raw) + +def create_cloud_xyz32(header, points): + """ + Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z). + @param header: The point cloud header. + @type header: L{std_msgs.msg.Header} + @param points: The point cloud points. + @type points: iterable + @return: The point cloud. + @rtype: L{sensor_msgs.msg.PointCloud2} + """ + fields = [PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + return create_cloud(header, fields, points) + +def _get_struct_fmt(is_bigendian, fields, field_names=None): + fmt = '>' if is_bigendian else '<' + offset = 0 + for field in (f for f in sorted(fields, key=lambda f: f.offset) if field_names is None or f.name in field_names): + if offset < field.offset: + fmt += 'x' * (field.offset - offset) + offset = field.offset + if field.datatype not in _DATATYPES: + print('Skipping unknown PointField datatype [%d]' % field.datatype, file=sys.stderr) + else: + datatype_fmt, datatype_length = _DATATYPES[field.datatype] + fmt += field.count * datatype_fmt + offset += field.count * datatype_length + + return fmt \ No newline at end of file diff --git a/src/obstacle_detector.py b/src/obstacle_detector.py new file mode 100755 index 0000000..8940caa --- /dev/null +++ b/src/obstacle_detector.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 + +""" +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Monash Nova Rover Team + +Subscriber node that finds the average Z value of +points with 0.3 < Z < 3.0, -0.15 < X < 0.15, and +-0.15 < Y < 0.15, then publishes this average to +obstacle_proximity topic as a float. Uses a custom +parser to parse PointCloud2 data object. +Refer cloud_point2.py +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +NODE: obstacle_detector +TOPICS: + - /D435/depth/color/points [sensor_msgs.msg.PointCloud2] + - /T265/odom/sample [nav_msgs.msg.Odometry] + - /obstacle_proximity [std_msgs.msg.Float32] +SERVICES: + - +ACTIONS: None +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +PACKAGE: induction task +AUTHOR(S): Amesh +CREATION: 27/09/2021 +EDITED: 01/09/2021 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +TODO: + - +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +""" + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import Odometry +import cloud_point2 as pc2 + + +# To create separate files set this variable to True +CREATE_SEPERATE = False +PC2_FILE = "sub1.csv" +ODO_FILE = "sub2.csv" + +class Publisher(Node): + def __init__(self): + """ + This class initializes a publisher. + Publisher topic - obstacle_proximity. + """ + super().__init__("obstacle_distance_publisher") + self.publisher = self.create_publisher(Float32, 'obstacle_proximity', 10) + + def publish_message(self, message: float): + """ + @param message: message to publish + @return: void + """ + msg = Float32() + msg.data = float(message) + self.publisher.publish(msg) + self.get_logger().info("Publishing: " + str(msg.data)) + + +class SubscribeAndPublish(Node): + def __init__(self): + """ + Initializes the publisher and the subscriber. + Subscriber topic - /D435/depth/color/points + """ + super().__init__('obstacle_detector') + self.publisher = Publisher() + self.file1 = open(PC2_FILE, "w") + if CREATE_SEPERATE : + self.file2 = open(ODO_FILE, "w") + self.file2.write("time,x\n") + self.file1.write("time,avg\n") + self.file1.close() + self.file2.close() + else: + self.file1.write("time,avg,x\n") + self.file1.close() + + + self.subscriber = self.create_subscription(PointCloud2, "/D435/depth/color/points", self.callback_func, 10) + self.subscriber_camera = self.create_subscription(Odometry, "/T265/odom/sample", self.callback_func2, 10) + self.subscriber_x = 0 + + def callback_func(self, msg: PointCloud2): + """ + This function is called when the publisher receives a message + @param msg: + @return: + """ + self.file1 = open(PC2_FILE, "a") + counter = 0 + total = 0.0 + # parse the pointcloud2 data set and returns list of lists. The inner list contains the data related to the field names + for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + if -0.15 < float(p[0]) < 0.15 and -0.15 < float(p[1]) < 0.15 and 0.3 < float(p[2]) < 3.0: + total += p[2] + counter += 1 + if counter > 0: + avg = total / counter + self.publisher.publish_message(float(avg)) + self.get_logger().info("Published message to publisher from subscriber: " + str(avg) + "for timestamp - " + str( + msg.header.stamp.sec) + " seconds") + # write data to csv file + if CREATE_SEPERATE: + self.file1.write(str(msg.header.stamp.sec)+ "," +str(avg) +"\n") + else: + self.file1.write(str(msg.header.stamp.sec)+ "," +str(avg)+ "," +str(self.subscriber_x) +"\n") + self.file1.close() + + + def callback_func2(self, msg: Odometry): + """ + This function is called when the publisher receives a message + @param msg: msg as a Odometry object + @return: + """ + if CREATE_SEPERATE: + self.file2 = open(ODO_FILE, "a") + self.file2.write( str(msg.header.stamp.sec)+ "," + str(msg.pose.pose.position.x) + "\n") + self.file2.close() + else: + self.subscriber_x = msg.pose.pose.position.x + + + +def main(): + rclpy.init() + subscriber = SubscribeAndPublish() + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()