diff --git a/.gitignore b/.gitignore index e7ea087..069f1d1 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,5 @@ # VIM *.swp +# Python +*.pyc diff --git a/CMakeLists.txt b/CMakeLists.txt index ea036b9..47f2b43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(nps_uw_sensors_gazebo) -find_package(catkin REQUIRED tf) +find_package(catkin REQUIRED tf gazebo_plugins) find_package(gazebo REQUIRED) find_package(roscpp REQUIRED) @@ -22,16 +22,25 @@ catkin_package() ## Plugins add_library(gazebo_ros_pulse_lidar_plugin src/gazebo_ros_pulse_lidar.cpp) +target_link_libraries(gazebo_ros_pulse_lidar_plugin + ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES}) + add_library(nps_gazebo_ros_gpu_sonar_single_beam_plugin src/nps_gazebo_ros_gpu_sonar_single_beam.cpp) - target_link_libraries(nps_gazebo_ros_gpu_sonar_single_beam_plugin - GpuRayPlugin ${catkin_LIBRARIES}) -target_link_libraries(gazebo_ros_pulse_lidar_plugin - ${GAZEBO_LIBRARIES} ${roscpp_LIBRARIES}) + GpuRayPlugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +add_library(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin + src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp + include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh) +target_link_libraries(nps_gazebo_ros_depth_camera_sonar_single_beam_plugin + DepthCameraPlugin ${catkin_LIBRARIES}) # for launch install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} PATTERN "*~" EXCLUDE) +# for Python scripts +catkin_install_python(PROGRAMS scripts/depth_camera_sonar.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh b/include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh new file mode 100644 index 0000000..1bb6c85 --- /dev/null +++ b/include/nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor. + * Author: John Hsu + * Date: 24 Sept 2008 + */ + +#ifndef GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH +#define GAZEBO_ROS_DEPTH_CAMERA_SONAR_SINGLE_BEAM_HH + +// ros stuff +#include +#include +#include + +// ros messages stuff +#include +#include +#include +#include +#include +#include + +// dynamic reconfigure stuff +#include +#include + +// boost stuff +#include + +// camera stuff +#include + +// gazebo stuff +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosDepthCamera(); + + /// \brief Destructor + public: ~GazeboRosDepthCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Advertise point cloud and depth image + public: virtual void Advertise(); + + /// \brief Update the controller + protected: virtual void OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewNormalsFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewRGBPointCloud(const float *_pcd, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Put camera data to the ROS topic + private: void FillPointdCloud(const float *_src); + + /// \brief push depth image data into ros topic + private: void FillDepthImage(const float *_src); + + /// \brief push normals image data into ros topic + private: void FillNormalsImage(const float *_src); + + /// \brief Keep track of number of connctions for point clouds + private: int point_cloud_connect_count_; + private: void PointCloudConnect(); + private: void PointCloudDisconnect(); + + /// \brief Keep track of number of connctions for Sonar point clouds + private: int sonar_point_cloud_connect_count_; + private: void SonarPointCloudConnect(); + private: void SonarPointCloudDisconnect(); + + /// \brief Keep track of number of connctions for images + private: int depth_image_connect_count_; + private: void DepthImageConnect(); + private: void DepthImageDisconnect(); + private: int normals_image_connect_count_; + private: void NormalsImageConnect(); + private: void NormalsImageDisconnect(); + private: void RayImageConnect(); + private: void RayImageDisconnect(); + private: common::Time last_depth_image_camera_info_update_time_; + + private: bool FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillDepthImageHelper(sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillNormalsImageHelper(sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + /// \brief A pointer to the ROS node. + /// A node will be instantiated if it does not exist. + private: ros::Publisher point_cloud_pub_; + + private: ros::Publisher depth_image_pub_; + private: ros::Publisher normals_image_pub_; + + /// \brief PointCloud2 point cloud message + private: sensor_msgs::PointCloud2 point_cloud_msg_; + + private: sensor_msgs::Image depth_image_msg_; + private: sensor_msgs::Image normals_image_msg_; + + private: double point_cloud_cutoff_; + + /// \brief ROS image topic name + private: std::string point_cloud_topic_name_; + + private: void InfoConnect(); + private: void InfoDisconnect(); + + using GazeboRosCameraUtils::PublishCameraInfo; + protected: virtual void PublishCameraInfo(); + + /// \brief image where each pixel contains the depth information + private: std::string depth_image_topic_name_; + private: std::string normals_image_topic_name_; + private: std::string depth_image_camera_info_topic_name_; + private: int depth_info_connect_count_; + private: void DepthInfoConnect(); + private: void DepthInfoDisconnect(); + + // overload with our own + private: common::Time depth_sensor_update_time_; + private: common::Time normals_sensor_update_time_; + protected: ros::Publisher depth_image_camera_info_pub_; + + private: event::ConnectionPtr load_connection_; + }; + +} +#endif + + diff --git a/launch/depth_camera_sonar_basic.launch b/launch/depth_camera_sonar_basic.launch new file mode 100644 index 0000000..65dc23e --- /dev/null +++ b/launch/depth_camera_sonar_basic.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scripts/depth_camera_sonar.py b/scripts/depth_camera_sonar.py new file mode 100755 index 0000000..261256b --- /dev/null +++ b/scripts/depth_camera_sonar.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python +#import sys +from argparse import ArgumentParser +from cv_bridge import CvBridge +import cv2 +import numpy as np +import rospy +from sensor_msgs.msg import Image + +# references: +# http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython +# http://docs.ros.org/melodic/api/cv_bridge/html/python/index.html + +# Notes: +# Arguments passed here from the launch file must match arguments in SDF. + +# subscriber name constants, should be replaced with sdf inputs +IMAGE_DEPTH_TOPIC = "depth_camera_sonar_sensor_camera/image_depth" +IMAGE_NORMALS_TOPIC = "depth_camera_sonar_sensor_camera/image_normals" + +# publisher name constants, should be replaced with sdf inputs +RAY_IMAGE_TOPIC = "sonar_ray_image" +RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" +BEAM_IMAGE_TOPIC = "sonar_beam_image" +BEAM_POINT_CLOUD_TOPIC = "sonar_beam_point_cloud" + +# get angle beam width, vert_count, horiz_count for one Sonar beam +def depth_camera_args(): + # args + parser = ArgumentParser(description="Start depth camera sonar ROS node") + parser.add_argument("beam_width", type=float, help="Beam width angle") + parser.add_argument("horiz_count", type=int, help="number of rays horiz") + parser.add_argument("vert_count", type=int, help="number of rays vert") + parser.add_argument("lobe_k1", type=float, help="Sonar lobe constant k1") + parser.add_argument("lobe_k2", type=float, help="Sonar lobe constant k2") + parser.add_argument("__name", type=str, help="launch adds name, unused") + parser.add_argument("__log", type=str, help="launch adds log, unused") + args = parser.parse_args() + return args.beam_width, args.horiz_count, args.vert_count, \ + args.lobe_k1, args.lobe_k2 + +# Given a horizontal beam width and the horizontal and vertical ray count, +# calculates the vertical beam width then iteratively sweeps through ray points +# from top to bottom to return the following index and angular information +# about each ray: horiz_index, vert_index, horiz_angle, vert_angle. +def ray_matrix_iterator(beam_width, horiz_count, vert_count): + beam_height = beam_width * vert_count / horiz_count + print("beam_width: %f, beam_height: %f, horiz_count %d, vert_count %d"%( + beam_width, beam_height, horiz_count, vert_count)) + + # set up horizontal and vertical starting angles and angle steps + if horiz_count < 2: + horiz_angle_0 = 0 + horiz_angle = 0 + else: + horiz_angle_0 = -beam_width/2.0 + horiz_angle = beam_width / (horiz_count - 1) + + if vert_count < 2: + height_0 = 0 + vert_angle = 0 + else: + height_0 = -beam_height/2.0 + vert_angle = beam_height / (vert_count - 1) + + # perform sweep + vert_angle = height_0 + for vert_index in range(vert_count): + horiz_angle = horiz_angle_0 + for horiz_index in range(horiz_count): + yield horiz_index, vert_index, horiz_angle, vert_angle # yield + horiz_angle += horiz_angle + vert_angle += vert_angle + +# calculate lobe power matrix +def lobe_power_matrix_constant(beam_width, horiz_count, vert_count, k1, k2): + + lobe_power_matrix = np.zeros((vert_count, horiz_count), np.float32) + + # put lobe power constant in each each element + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): + ray_power = 1.0 # should calculate using angles and constants + lobe_power_matrix[row,column] = ray_power + + return lobe_power_matrix + +# currently retro power is hardcoded to 1 +def retro_power_matrix_constant(vert_count, horiz_count): + retro_matrix = np.ones((vert_count, horiz_count), np.float32) + return retro_matrix + +# calculate depth power matrix from depth values +def calculate_depth_power_matrix(depth_matrix, + beam_width, horiz_count, vert_count): + depth_power_matrix = np.zeros((vert_count, horiz_count), np.float32) + + # put depth power in each each element + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): + depth = depth_matrix[row,column] + depth_power = depth # use equation instead + depth_power_matrix[row, column] = depth_power + return depth_power_matrix + +def calculate_normals_power_matrix(normals_f4, + beam_width, horiz_count, vert_count): + normals_power_matrix = np.zeros((vert_count, horiz_count), np.float32) + + # put normals angle tuple in each each element + for column, row, horizontal_angle, vertical_angle in ray_matrix_iterator( + beam_width, horiz_count, vert_count): + + # calculate horizontal and vertical angles from x,y,z,1 + x,y,z,_unused = normals_f4[row, column] + horizontal_normal = 0.0 # replace with calculation that uses xyz + vertical_normal = 0.0 + horizontal_normal += horizontal_angle + vertical_normal += vertical_angle + + # apply math on vertical and horizontal normals to get normals power + normals_power_matrix[row, column] = 1.0 + + return normals_power_matrix + +# calculate the ray power matrix from the power matrices +def powers_to_rays(depth_power_matrix, normals_power_matrix, + lobe_power_matrix, retro_power_matrix): + + # rays + ray_power_matrix = cv2.multiply(depth_power_matrix, normals_power_matrix) + ray_power_matrix = cv2.multiply(ray_power_matrix, lobe_power_matrix) + ray_power_matrix = cv2.multiply(ray_power_matrix, retro_power_matrix) + return ray_power_matrix + +class SonarNode: + def __init__(self): + + # user inputs from the launch file. They should match sdf. + self.beam_width, self.horiz_count, self.vert_count, \ + self.lobe_k1, self.lobe_k2 = depth_camera_args() + + # depth and normals power matrices from depth and normals from GPU + self.depth_matrix = None + self.depth_power_matrix = None + self.normals_power_matrix = None + + # lobe power matrix + self.lobe_power_matrix = lobe_power_matrix_constant( + self.beam_width, self.horiz_count, self.vert_count, + self.lobe_k1, self.lobe_k2) + + # retro power matrix should be calculated but retro power is hardcoded + self.retro_power_matrix = np.ones( + (self.vert_count, self.horiz_count), np.float32) + + # ROS subscribers + self.depth_sub = rospy.Subscriber(IMAGE_DEPTH_TOPIC, + Image, self.on_depth_image) + self.normals_sub = rospy.Subscriber(IMAGE_NORMALS_TOPIC, + Image, self.on_normals_image) + + # ROS publishers + self.ray_pub = rospy.Publisher(RAY_IMAGE_TOPIC, Image, queue_size=10) + + # ROS-CV bridge + self.bridge = CvBridge() + + # calculate depth power matrix from Gazebo depth_image and cache it + def on_depth_image(self, depth_image): + rospy.loginfo(rospy.get_caller_id() + " received depth_image") + self.depth_matrix = self.bridge.imgmsg_to_cv2(depth_image) + print("depth image shape: ", self.depth_matrix.shape) + if self.depth_matrix.shape != (self.vert_count, self.horiz_count): + # bad + print("Invalid depth image shape", self.depth_matrix.shape) + return + + self.depth_power_matrix = calculate_depth_power_matrix( + self.depth_matrix, + self.beam_width, self.horiz_count, self.vert_count) + + # calculate normals power matrix from Gazebo normals_image + # then calculate ray power and beam power + def on_normals_image(self, normals_image): + rospy.loginfo(rospy.get_caller_id() + " received normals_image") + normals_matrix_f4 = self.bridge.imgmsg_to_cv2(normals_image) + print("normals image shape: ", normals_matrix_f4.shape) + if normals_matrix_f4.shape != (self.vert_count, self.horiz_count, 4): + # bad + print("Invalid normals image shape", normals_matrix_f4.shape) + return + + self.normals_power_matrix = calculate_normals_power_matrix( + normals_matrix_f4, + self.beam_width, self.horiz_count, self.vert_count) + + # generate all the outputs + self.generate_outputs() + + # generate all the outputs + def generate_outputs(self): + + # use depth and normals matrices to make rays + ray_power_matrix = powers_to_rays(self.depth_power_matrix, + self.normals_power_matrix, + self.lobe_power_matrix, + self.retro_power_matrix) + + # advertise ray_power_matrix to ROS, inheriting the 32FC1 format + self.ray_pub.publish(self.bridge.cv2_to_imgmsg(ray_power_matrix, + "passthrough")) +# print("Ray power matrix", ray_power_matrix) + +# # create and publish the ray point cloud +# ray_cloud = ray_point_cloud(ray_power_matrix, +# self.beam_width, self.horiz_count, self.vert_count) +# self.ray_cloud_pub.publish(ray_cloud) +# +# # create and publish the beam point cloud +# beam_cloud = beam_point_cloud(ray_power_matrix, +# self.beam_width, self.horiz_count, self.vert_count) +# self.beam_cloud_pub.publish(ray_cloud) + +if __name__ == '__main__': + rospy.init_node('depth_and_normals_to_sonar', anonymous=True) + ros_sonar_node = SonarNode() + + # spin until Ctrl-C + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + diff --git a/scripts/sonar_equations.py b/scripts/sonar_equations.py new file mode 100755 index 0000000..ae5966a --- /dev/null +++ b/scripts/sonar_equations.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python +# We use Python 2 instead of python3 bacause ROS uses Python 2. +# ref. https://numpy.org/doc/1.18/numpy-user.pdf + +# Single Beam Sonar +# Sonar Point-Scatter Model +# Contributors: Andreina Rascon, Derek Olson, Woeng-Sug Choi + +from random import random +from math import sqrt, sin, cos, pi, log, acos +import numpy as np +import matplotlib.pyplot as plt +import rospy + +# various diagnostics for beam 0 +def _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2): + + # Plots + plt.figure(figsize=(14,10), dpi=80) + plt.suptitle("%d beam(s), %d elevation rays, %d azimuth rays " + "%d frequencies, %d buckets"%(nBeams, + ray_nElevationRays, ray_nAzimuthRays, nFreq, nBuckets)) + + # inverse fast fourier transform + plt.subplot(2,2,1) + plt.title("Power based on echo time") + plt.grid(True) + plt.plot(time1f, P_beam_tf2[0,:], linewidth=0.5) + plt.xlabel('Time, [s]') + plt.ylabel('Pressure, [Pa]') + + # Sound Pressure Level of Echo Level + SPLf1 = 20 * np.log(np.abs(P_beam_tf2[0,:])) # sound pressure level, [dB] + plt.subplot(2,2,2) + plt.title("Sound pressure level based on echo time") + plt.grid(True) + plt.plot(time1f, SPLf1, linewidth=0.5) + plt.xlabel('Time, [s]') + plt.ylabel('Sound Pressure Level, [Pa]') + + # image for each nFreq + plt.subplot(2,2,3) + plt.title("Heatplot sound pressure level (SPL) based on frequency number") + plt.xlabel('Beam number') + plt.ylabel('Inverse FFT frequency number') + plt.imshow(P_beam_tf2.T, aspect="auto") + + # image for each nBuckets + plt.subplot(2,2,4) + plt.title("Bucketed heatplot SPL based on bucketed frequency number") + plt.xlabel('Beam number') + plt.ylabel('Inverse FFT frequency bucket number') + plt.imshow(P_bucket_tf2.T, aspect="auto") + + plt.show() + +# diagnostics showing pressure levels for each beam +def _show_plots_powers(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2): + + # Plots + plt.figure(figsize=(14,10), dpi=80) + plt.suptitle("Sound pressure level in Db based on echo time\n" + "%d beam(s), %d elevation rays, %d azimuth rays " + "%d frequencies"%(nBeams, + ray_nElevationRays, ray_nAzimuthRays, nFreq)) + + # Sound Pressure Level of Echo Level + for i in range(16): + SPLf1 = 20 * np.log(np.abs(P_beam_tf2[i,:])) +# SPLf1 = 20 * np.log(np.abs(P_bucket_tf2[i,:])) # bucketed + plt.subplot(16,1,i+1) + plt.grid(True) + plt.plot(time1f, SPLf1, linewidth=0.5) + plt.xlabel('Time, [s]') + plt.show() + +# unnormalized sinc function +def _unnormalized_sinc(t): + try: + return sin(t)/t + except ZeroDivisionError: + return 1.0 + +## physics constants +soundSpeed = 1500.0 # [m/s] +mu = 10e-4 # Surface Reflectivity + +# Input Parameters +# The BlueView P900-45 is used as an example sonar for the purposes of +# demonstrating the point-scatter model + +# Sonar properties +sonarFreq = 900E3 # Sonar frquency +bandwidth = 29.5e4 # [Hz] +freqResolution = 100e2 +fmin = sonarFreq - bandwidth/2*4 # Calculated requency spectrum +fmax = sonarFreq + bandwidth/2*4 # Calculated requency spectrum + +def _textf3(text, f): + return "%s: %f, %f, %f"%(text,f[0],f[1],f[2]) + +# incidence angle is target's normal angle accounting for the ray's azimuth +# and elevation +def _ray_incidence(azimuth, elevation, normalf4): + # ray normal from camera azimuth and elevation + camera_x = cos(-azimuth)*cos(elevation) + camera_y = sin(-azimuth)*cos(elevation) + camera_z = sin(elevation) + ray_normal = np.array([camera_x, camera_y, camera_z]) + + # target normal with axes compensated to camera axes + target_normal = np.array([normalf4[2], -normalf4[0], -normalf4[1]]) + + # dot product + dot_product = ray_normal.dot(target_normal) + return pi - acos(dot_product) + +def process_rays(ray_distancesf2, ray_normalsf2_4, show_plots=False): + + # Sonar sensor properties + nBeams = 16 + beam_elevationAngle = 0.0175 # Beam looking down in elevation direction + beam_azimuthAngle = 0.0 # Beam at center line in azimuth direction + beam_elevationAngleWidth = 0.1 # radians + beam_azimuthAngleWidth = 0.1 # radians + ray_nElevationRays = 4 + ray_nAzimuthRays = 3 + + nBuckets = 300 + + if ray_distancesf2.shape != (ray_nElevationRays, ray_nAzimuthRays * nBeams): + print("bad distances shape ", ray_distancesf2.shape) + return np.zeros(nBeams,nBuckets) + + # calculated Sonar sensor properties + ray_elevationAnglesf1 = beam_elevationAngle + np.linspace( + -beam_elevationAngleWidth / 2, beam_elevationAngleWidth / 2, + ray_nElevationRays) + ray_azimuthAnglesf1 = beam_azimuthAngle + np.linspace( + -beam_azimuthAngleWidth / 2, beam_azimuthAngleWidth / 2, + ray_nAzimuthRays) + ray_elevationAngleWidth = beam_elevationAngleWidth/(ray_nElevationRays - 1) + ray_azimuthAngleWidth = beam_azimuthAngleWidth/(ray_nAzimuthRays - 1) + + # calculated sampling periods + max_T = np.amax(ray_distancesf2)*2/soundSpeed + _delta_f = 1/max_T + + # _delta_t = 1/(fmax - fmin) + nFreq = int(round((fmax - fmin) / _delta_f)) + + # reduce nFreq because calculated nFreq is too large for looping + print("nFreq", nFreq) + _freq1f = np.linspace(fmin,fmax,nFreq) + + # calculated physics + _absorption = 0.0354 # [dB/m] + _attenuation = _absorption*log(10)/20 + _kw1f = 2*pi*_freq1f/soundSpeed # wave vector + K1f = _kw1f + 1j*_attenuation # attenuation constant K1f + + # Transmit spectrum, frequency domain + S_f1f = 1e11 * np.exp(-(_freq1f - sonarFreq)**2 * pi**2 / bandwidth**2) + + # Point Scattering model + # Echo level using the point scatter model for P(f) and P(t) for beams + P_ray_f2c = np.zeros((nBeams, nFreq), dtype=np.complex_) + azimuthBeamPattern2f = np.zeros((ray_nElevationRays,ray_nAzimuthRays)) + elevationBeamPattern2f = np.zeros((ray_nElevationRays,ray_nAzimuthRays)) + for k in range(ray_nElevationRays): + for i in range(ray_nAzimuthRays): + azimuthBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 + / ray_azimuthAngleWidth * sin(ray_azimuthAnglesf1[i]))))**2 + elevationBeamPattern2f[k,i] = (abs(_unnormalized_sinc(pi * 0.884 + / ray_elevationAngleWidth * sin(ray_elevationAnglesf1[k]))))**2 + + # diagnostics image of ray incidences + incidences_f2 = np.zeros((ray_nElevationRays, ray_nAzimuthRays * nBeams), + dtype=np.float32) # diagnostics message + + for k in range(ray_nElevationRays): + for i in range(ray_nAzimuthRays * nBeams): + xi_z = random() # generate a random number, (Gaussian noise) + xi_y = random() # generate another random number, (Gaussian noise) +# xi_z = 0.5 # turn off randomness +# xi_y = 0.5 # turn off randomness + + # ray r in beam i + r = i % ray_nAzimuthRays + + # angle between ray vector and object normal vector + incidence = _ray_incidence(ray_azimuthAnglesf1[r], + ray_elevationAnglesf1[k], + ray_normalsf2_4[k, i]) + incidences_f2[k,i] = incidence + + distance = ray_distancesf2[k,i] + amplitude = (((xi_z + 1j * xi_y) + / sqrt(2)) + * (sqrt(mu * cos(incidence)**2 * distance**2 + * ray_azimuthAngleWidth + * ray_elevationAngleWidth)) + * azimuthBeamPattern2f[k,r] + * elevationBeamPattern2f[k,r]) + + # Summation of Echo returned from a signal (frequency domain) + b = int(i/ray_nAzimuthRays) # beam + for m in range(nFreq): + P_ray_f2c[b,m] = P_ray_f2c[b,m] + S_f1f[m] * amplitude \ + * np.exp(-1j * K1f[m] * distance * 2) / (distance**2) + + # power level based on echo time for each beam + P_beam_tf2 = np.zeros((nBeams, nFreq), dtype=np.float32) + for b in range(nBeams): + P_beam_tf2[b,:] = np.fft.ifft(P_ray_f2c[b,:]) + + # power into buckets + P_bucket_tf2 = np.zeros((nBeams, nBuckets), dtype=np.float32) + for b in range(nBeams): + for f in range(nFreq): + bucket = int(f*nBuckets/nFreq) + P_bucket_tf2[b, bucket] += P_beam_tf2[b,f] + +# show_plots = True + if show_plots: + time1f = np.linspace(0,max_T,nFreq) # for diagnostics plot +# _show_plots(nBeams, ray_nElevationRays, ray_nAzimuthRays, +# nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) + _show_plots_powers(nBeams, ray_nElevationRays, ray_nAzimuthRays, + nFreq, nBuckets, time1f, P_beam_tf2, P_bucket_tf2) + +# return P_beam_tf2.T, incidences_f2 # unbucketed beam + return P_bucket_tf2.T, incidences_f2 # bucketed beam + +# test +if __name__ == '__main__': + # These dimensions must match hardcoded dimensions + # 16 beams 3 wide 4 tall + ray_distancesf2 = np.zeros((4,48), dtype=np.float32) + ray_distancesf2[:,] = np.linspace(0.5, 6.0, 48) + ray_normalsf2_4 = np.zeros((4,48,4), dtype=np.float32) + ray_normalsf2_4[:,:,0]=1.0 + + print("ray_distancesf2", ray_distancesf2) + print("ray_normalsf2_4", ray_normalsf2_4) + + # run test dataset and show plots + _image, _incidences = process_rays(ray_distancesf2, ray_normalsf2_4, True) + diff --git a/scripts/sonar_ros.py b/scripts/sonar_ros.py new file mode 100755 index 0000000..d82db7c --- /dev/null +++ b/scripts/sonar_ros.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python +#import sys +from argparse import ArgumentParser +from cv_bridge import CvBridge +import cv2 +import numpy as np +import rospy +from sensor_msgs.msg import Image + +from sonar_equations import process_rays + +# references: +# http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython +# http://docs.ros.org/melodic/api/cv_bridge/html/python/index.html + +# subscriber name constants, should be replaced with sdf inputs +IMAGE_DEPTH_TOPIC = "depth_camera_sonar_sensor_camera/image_depth" +IMAGE_NORMALS_TOPIC = "depth_camera_sonar_sensor_camera/image_normals" + +# publisher name constants, should be replaced with sdf inputs +#RAY_IMAGE_TOPIC = "sonar_ray_image" +#RAY_POINT_CLOUD_TOPIC = "sonar_ray_point_cloud" +BEAM_IMAGE_TOPIC = "sonar_beam_image" +#BEAM_POINT_CLOUD_TOPIC = "sonar_beam_point_cloud" +INCIDENCE_IMAGE_TOPIC = "sonar_ray_incidence_image" + +class SonarNode: + def __init__(self): + + # ROS subscribers + self.depth_sub = rospy.Subscriber(IMAGE_DEPTH_TOPIC, + Image, self.on_depth_image) + self.normals_sub = rospy.Subscriber(IMAGE_NORMALS_TOPIC, + Image, self.on_normals_image) + + # ROS publishers + self.beam_pub = rospy.Publisher(BEAM_IMAGE_TOPIC, Image, queue_size=10) + self.incidence_pub = rospy.Publisher(INCIDENCE_IMAGE_TOPIC, Image, queue_size=10) + + # ROS-CV bridge + self.bridge = CvBridge() + + # calculate depth power matrix from Gazebo depth_image and cache it + def on_depth_image(self, depth_image): + rospy.loginfo(rospy.get_caller_id() + " received depth_image") + self.depth_matrix = self.bridge.imgmsg_to_cv2(depth_image) + print("depth image shape: ", self.depth_matrix.shape) + + # calculate normals power matrix from Gazebo normals_image + # then calculate ray power and beam power + def on_normals_image(self, normals_image): + rospy.loginfo(rospy.get_caller_id() + " received normals_image") + self.normals_matrix_f4 = self.bridge.imgmsg_to_cv2(normals_image) + print("normals image shape: ", self.normals_matrix_f4.shape) + + # generate all the outputs + beam_matrix, alphas = process_rays(self.depth_matrix, + self.normals_matrix_f4) + + # advertise the bucketed beam power matrix + self.beam_pub.publish(self.bridge.cv2_to_imgmsg(beam_matrix, + "passthrough")) + + # advertise alphas to ROS for diagnostics + self.incidence_pub.publish(self.bridge.cv2_to_imgmsg(alphas, + "passthrough")) + +if __name__ == '__main__': + rospy.init_node('depth_and_normals_to_sonar', anonymous=True) + ros_sonar_node = SonarNode() + + # spin until Ctrl-C + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + diff --git a/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp new file mode 100644 index 0000000..edf0587 --- /dev/null +++ b/src/nps_gazebo_ros_depth_camera_sonar_single_beam.cpp @@ -0,0 +1,754 @@ +/* + * Copyright 2013 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + Desc: GazeboRosDepthCamera plugin for simulating cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 + */ + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "nps_uw_sensors_gazebo/nps_gazebo_ros_depth_camera_sonar_single_beam.hh" + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosDepthCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosDepthCamera::GazeboRosDepthCamera() +{ + this->point_cloud_connect_count_ = 0; + this->depth_image_connect_count_ = 0; + this->normals_image_connect_count_ = 0; + this->depth_info_connect_count_ = 0; + this->point_cloud_cutoff_ = 0.4; + this->sonar_point_cloud_connect_count_ = 0; + this->last_depth_image_camera_info_update_time_ = common::Time(0); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosDepthCamera::~GazeboRosDepthCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf) +{ + std::cout << "nps_gazebo_ros_depth_camera_sonar_single_beam"; + DepthCameraPlugin::Load(_parent, _sdf); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("depth_camera", "A ROS node for Gazebo has not " + "been initialized, unable to load plugin. Load the " + "Gazebo system plugin 'libgazebo_ros_api_plugin.so' " + "in the gazebo_ros package)"); + return; + } + + // copying from DepthCameraPlugin into GazeboRosCameraUtils + this->parentSensor_ = this->parentSensor; + this->width_ = this->width; + this->height_ = this->height; + this->depth_ = this->depth; + this->format_ = this->format; + this->camera_ = this->depthCamera; + + // using a different default + if (!_sdf->HasElement("imageTopicName")) + this->image_topic_name_ = "ir/image_raw"; + if (!_sdf->HasElement("cameraInfoTopicName")) + this->camera_info_topic_name_ = "ir/camera_info"; + + // point cloud stuff + if (!_sdf->HasElement("pointCloudTopicName")) + this->point_cloud_topic_name_ = "points"; + else + this->point_cloud_topic_name_ = + _sdf->GetElement("pointCloudTopicName")->Get(); + + // depth image and normals stuff + if (!_sdf->HasElement("depthImageTopicName")) + this->depth_image_topic_name_ = "depth/image_raw"; + else + this->depth_image_topic_name_ = + _sdf->GetElement("depthImageTopicName")->Get(); + + if (!_sdf->HasElement("normalsImageTopicName")) + this->normals_image_topic_name_ = "normals/image_raw"; + else + this->normals_image_topic_name_ = + _sdf->GetElement("normalsImageTopicName")->Get(); + + if (!_sdf->HasElement("depthImageCameraInfoTopicName")) + this->depth_image_camera_info_topic_name_ = "depth/camera_info"; + else + this->depth_image_camera_info_topic_name_ = + _sdf->GetElement("depthImageCameraInfoTopicName")->Get(); + + if (!_sdf->HasElement("pointCloudCutoff")) + this->point_cloud_cutoff_ = 0.4; + else + this->point_cloud_cutoff_ = + _sdf->GetElement("pointCloudCutoff")->Get(); + + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind( + &GazeboRosDepthCamera::Advertise, this)); + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +void GazeboRosDepthCamera::Advertise() +{ + ros::AdvertiseOptions point_cloud_ao = + ros::AdvertiseOptions::create( + this->point_cloud_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::PointCloudConnect, this), + boost::bind(&GazeboRosDepthCamera::PointCloudDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); + + ros::AdvertiseOptions depth_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->depth_image_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::DepthImageConnect, this), + boost::bind(&GazeboRosDepthCamera::DepthImageDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao); + + ros::AdvertiseOptions normals_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->normals_image_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::NormalsImageConnect, this), + boost::bind(&GazeboRosDepthCamera::NormalsImageDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + this->normals_image_pub_ = this->rosnode_->advertise(normals_image_ao); + + ros::AdvertiseOptions depth_image_camera_info_ao = + ros::AdvertiseOptions::create( + this->depth_image_camera_info_topic_name_, 1, + boost::bind(&GazeboRosDepthCamera::DepthInfoConnect, this), + boost::bind(&GazeboRosDepthCamera::DepthInfoDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_camera_info_pub_ = + this->rosnode_->advertise(depth_image_camera_info_ao); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::PointCloudConnect() +{ + this->point_cloud_connect_count_++; + (*this->image_connect_count_)++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::PointCloudDisconnect() +{ + this->point_cloud_connect_count_--; + (*this->image_connect_count_)--; + if (this->point_cloud_connect_count_ <= 0) + this->parentSensor->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthImageConnect() +{ +std::cout << "DepthImageConnect\n"; + this->depth_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthImageDisconnect() +{ +std::cout << "DepthImageDisconnect\n"; + this->depth_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::NormalsImageConnect() +{ +std::cout << "NormalsImageConnect\n"; + this->normals_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::NormalsImageDisconnect() +{ +std::cout << "NormalsImageDisconnect\n"; + this->normals_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthInfoConnect() +{ + this->depth_info_connect_count_++; +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthInfoDisconnect() +{ + this->depth_info_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ +std::cout << "OnNewDepthFrame\n"; + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->depth_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); +# else + this->depth_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); +# endif + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + this->normals_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { + if (this->point_cloud_connect_count_ > 0) + this->FillPointdCloud(_image); + + if (this->depth_image_connect_count_ > 0) + this->FillDepthImage(_image); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0 || // zz why? + this->normals_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 + // frame after activate + this->parentSensor->SetActive(true); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ +std::cout << "OnNewRGBPointCloud\n"; + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->depth_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); +# else + this->depth_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); +# endif + + if (!this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ > 0) + // do this first so there's chance for sensor to run 1 + // frame after activate + this->parentSensor->SetActive(true); + } + else + { + if (this->point_cloud_connect_count_ > 0) + { + this->lock_.lock(); + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = + this->point_cloud_msg_.point_step * this->width; + + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(_width*_height); + + point_cloud_msg_.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + for (unsigned int i = 0; i < _width; i++) + { + for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, + ++iter_z, ++iter_rgb) + { + unsigned int index = (j * _width) + i; + *iter_x = _pcd[4 * index]; + *iter_y = _pcd[4 * index + 1]; + *iter_z = _pcd[4 * index + 2]; + *iter_rgb = _pcd[4 * index + 3]; + if (i == _width /2 && j == _height / 2) + { + uint32_t rgb = *reinterpret_cast(&(*iter_rgb)); + uint8_t r = (rgb >> 16) & 0x0000ff; + uint8_t g = (rgb >> 8) & 0x0000ff; + uint8_t b = (rgb) & 0x0000ff; + std::cerr << static_cast(r) << " " << static_cast(g) + << " " << static_cast(b) << std::endl; + } + } + } + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + this->lock_.unlock(); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ +std::cout << "OnNewImageFrame\n"; + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + // ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s", + // this->parentSensor_->GetName().c_str(), + // this->frame_name_.c_str()); + +# if GAZEBO_MAJOR_VERSION >= 7 + this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); +# else + this->sensor_update_time_ = this->parentSensor->GetLastMeasurementTime(); +# endif + + if (!this->parentSensor->IsActive()) + { + if ((*this->image_connect_count_) > 0) + // do this first so there's chance for sensor to run 1 + // frame after activate + this->parentSensor->SetActive(true); + } + else + { + if ((*this->image_connect_count_) > 0) + { + this->PutCameraData(_image); + // TODO(lucasw) publish camera info with depth image + // this->PublishCameraInfo(sensor_update_time); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put camera data to the interface +void GazeboRosDepthCamera::FillPointdCloud(const float *_src) +{ + this->lock_.lock(); + + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = + this->point_cloud_msg_.point_step * this->width; + + // copy from depth to point cloud message + FillPointCloudHelper(this->point_cloud_msg_, + this->height, + this->width, + this->skip_, + const_cast(reinterpret_cast(_src))); + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + + this->lock_.unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Put depth image data to the interface +void GazeboRosDepthCamera::FillDepthImage(const float *_src) +{ + this->lock_.lock(); + // copy data into image + this->depth_image_msg_.header.frame_id = this->frame_name_; + this->depth_image_msg_.header.stamp.sec = + this->depth_sensor_update_time_.sec; + this->depth_image_msg_.header.stamp.nsec = + this->depth_sensor_update_time_.nsec; + + // copy from depth to depth image message + FillDepthImageHelper(this->depth_image_msg_, + this->height, + this->width, + this->skip_, + const_cast(reinterpret_cast(_src))); + + this->depth_image_pub_.publish(this->depth_image_msg_); + + this->lock_.unlock(); +} + +// Fill depth information +bool GazeboRosDepthCamera::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg.is_bigendian = 0; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = reinterpret_cast(&(image_msg.data[0])); + float* toCopyFrom = reinterpret_cast(data_arg); + int index = 0; + + // convert depth to image + std::cout << "FillDepthImageHelper rows: " << rows_arg + << " cols: " << cols_arg << std::endl; + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + if (depth > this->point_cloud_cutoff_) + { + dest[i + j * cols_arg] = depth; + } + else // point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + } + return true; +} + +// Fill depth information +bool GazeboRosDepthCamera::FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(rows_arg*cols_arg); + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + point_cloud_msg.is_dense = true; + + float* toCopyFrom = reinterpret_cast(data_arg); + int index = 0; + + double hfov = this->parentSensor->DepthCamera()->HFOV().Radian(); + double fl = (static_cast(this->width)) / (2.0 *tan(hfov/2.0)); + + // convert depth to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + double pAngle; + if (rows_arg > 1) + { + pAngle = atan2(static_cast(j) - + 0.5*static_cast(rows_arg-1), fl); + } + else + { + pAngle = 0.0; + } + + for (uint32_t i = 0; i < cols_arg; i++, ++iter_x, ++iter_y, + ++iter_z, ++iter_rgb) + { + double yAngle; + if (cols_arg > 1) + { + yAngle = atan2(static_cast(i) - + 0.5*static_cast(cols_arg-1), fl); + } + else + { + yAngle = 0.0; + } + + double depth = toCopyFrom[index++]; + + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); + if (depth > this->point_cloud_cutoff_) + { + *iter_z = depth; + } + else // point in the unseeable range + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); + point_cloud_msg.is_dense = false; + } + + // put image color data for each point + uint8_t* image_src = + reinterpret_cast(&(this->image_msg_.data[0])); + + if (this->image_msg_.data.size() == rows_arg*cols_arg*3) + { + // color + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; + } + else if (this->image_msg_.data.size() == rows_arg*cols_arg) + { + // mono (or bayer? @todo; fix for bayer) + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; + } + else + { + // no image + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; + } + } + } + + return true; +} + +// ************************************************************************ +// normals +// ************************************************************************ + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewNormalsFrame(const float *_normals_image, + unsigned int _width, unsigned int _height, unsigned int _normals, + const std::string &_format) +{ +std::cout << "OnNewNormalsFrame\n"; + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + +# if GAZEBO_MAJOR_VERSION >= 7 + this->normals_sensor_update_time_ = + this->parentSensor->LastMeasurementTime(); +# else + this->normals_sensor_update_time_ = + this->parentSensor->GetLastMeasurementTime(); +# endif + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + this->normals_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { +// if (this->point_cloud_connect_count_ > 0) +// this->FillPointdCloud(_normals_image); + + if (this->normals_image_connect_count_ > 0) + this->FillNormalsImage(_normals_image); + +// zz // lets perform Sonar calculations here +// zz this->FillRayImage(); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0 || // zz why? + this->normals_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 + // frame after activate + this->parentSensor->SetActive(true); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put normals image data to the interface +void GazeboRosDepthCamera::FillNormalsImage(const float *_src) +{ + this->lock_.lock(); + // copy data into image + this->normals_image_msg_.header.frame_id = this->frame_name_; + this->normals_image_msg_.header.stamp.sec = + this->normals_sensor_update_time_.sec; + this->normals_image_msg_.header.stamp.nsec = + this->normals_sensor_update_time_.nsec; + + // copy from normals to normals image message + FillNormalsImageHelper(this->normals_image_msg_, + this->height, + this->width, + this->skip_, + const_cast(reinterpret_cast(_src))); + + this->normals_image_pub_.publish(this->normals_image_msg_); + + this->lock_.unlock(); +} + +// Fill normals information +bool GazeboRosDepthCamera::FillNormalsImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + image_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC4; + image_msg.height = rows_arg; + image_msg.width = cols_arg; + image_msg.step = 4 * sizeof(float) * cols_arg; + image_msg.data.resize(rows_arg * cols_arg * sizeof(float) * 4); + image_msg.is_bigendian = 0; + + float* dest = reinterpret_cast(&(image_msg.data[0])); + float* toCopyFrom = reinterpret_cast(data_arg); + + // accept values as they are so memcpy without looped checks + memcpy(dest, toCopyFrom, rows_arg * cols_arg * 4 * sizeof(float)); + + return true; +} + +// ************************************************************************ +// end normals +// ************************************************************************ + + +void GazeboRosDepthCamera::PublishCameraInfo() +{ + ROS_DEBUG_NAMED("depth_camera", "publishing default camera info," + " then depth camera info"); + GazeboRosCameraUtils::PublishCameraInfo(); + + if (this->depth_info_connect_count_ > 0) + { +# if GAZEBO_MAJOR_VERSION >= 7 + common::Time sensor_update_time = + this->parentSensor_->LastMeasurementTime(); +# else + common::Time sensor_update_time = + this->parentSensor_->GetLastMeasurementTime(); +# endif + this->sensor_update_time_ = sensor_update_time; + if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= + this->update_period_) + { +// this->PublishCameraInfo(this->depth_image_camera_info_pub_ , +// sensor_update_time); + this->PublishCameraInfo(this->depth_image_camera_info_pub_); + this->last_depth_image_camera_info_update_time_ = sensor_update_time; + } + } +} + +// @todo: publish disparity similar to +// openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +/* +#include +pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); + +void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time) +{ + stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared (); + disp_msg->header.stamp = time; + disp_msg->header.frame_id = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + disp_msg->image.height = depth_height_; + disp_msg->image.width = depth_width_; + disp_msg->image.step = disp_msg->image.width * sizeof (float); + disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step); + disp_msg->T = depth.getBaseline (); + disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth (); + + /// @todo Compute these values from DepthGenerator::GetDeviceMaxDepth() and the like + disp_msg->min_disparity = 0.0; + disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3; + disp_msg->delta_d = 0.125; + + depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast(&disp_msg->image.data[0]), disp_msg->image.step); + + pub_disparity_.publish (disp_msg); +} +*/ + + +} + diff --git a/urdf/depth_camera_sonar.xacro b/urdf/depth_camera_sonar.xacro new file mode 100644 index 0000000..eecd99e --- /dev/null +++ b/urdf/depth_camera_sonar.xacro @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + false + + + true + ${update_rate} + true + ${topic} + 0 0 0 0 0 0 + + + depth_camera_sonar_sensor_camera + + image_raw + image_raw/camera_info + point_cloud + sonar_point_cloud + image_depth + image_normals + image_ray + image_depth/camera_info + + forward_sonar_optical_link + + + + ${beam_width} + + ${horiz_count * num_beams} + ${vert_count} + R8G8B8 + + + ${min_range} + ${max_range} + + + depths normals + + + + + + + diff --git a/urdf/depth_camera_sonar_robot.xacro b/urdf/depth_camera_sonar_robot.xacro new file mode 100644 index 0000000..e15faa5 --- /dev/null +++ b/urdf/depth_camera_sonar_robot.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + diff --git a/worlds/sonar_single_beam_basic.world b/worlds/depth_camera_sonar_basic.world similarity index 100% rename from worlds/sonar_single_beam_basic.world rename to worlds/depth_camera_sonar_basic.world