From 0965130c1d93c02cfe366b2f396d1d212939990b Mon Sep 17 00:00:00 2001 From: rostest Date: Mon, 28 Oct 2024 15:22:10 +0100 Subject: [PATCH] Fix TiM7xx fieldset settings and services, #394, #408 --- CONTRIBUTING.md | 54 +- driver/src/sick_scan_marker.cpp | 1097 ++++----- driver/src/sick_scan_services.cpp | 2351 ++++++++++---------- include/sick_scan/sick_generic_field_mon.h | 424 ++-- include/sick_scan/sick_scan_marker.h | 246 +- 5 files changed, 2108 insertions(+), 2064 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index ade1891..1a543b8 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -75,6 +75,38 @@ of this file. # Bloom release +Summary of bloom release build: + +* Update release repositories for rosdistros noetic, humble, iron and jazzy with `bloom-release` on Linux: + ``` + cd /tmp + bloom-release --rosdistro noetic -d sick_scan_xd # update release repository https://github.com/SICKAG/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro humble -d sick_scan_xd # update release repository https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro iron -d sick_scan_xd # update release repository https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro jazzy -d sick_scan_xd # update release repository https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + ``` + Confirm "push to release" and "open pull request" with Y. + If `bloom-release` is not installed, then install it by `sudo apt-get install python-bloom`. + +* Check that the new version is committed in the release repositories https://github.com/SICKAG/sick_scan_xd-release.git (ROS 1) and https://github.com/ros2-gbp/sick_scan_xd-release.git (ROS 2). + +* Check Jenkins build status (new Jenkins build after 0-3 days): + * ROS 1 noetic jenkins build status: https://build.ros.org/job/Ndev__sick_scan_xd__ubuntu_focal_amd64/lastBuild/ + * ROS 2 humble jenkins build status: https://build.ros2.org/job/Hdev__sick_scan_xd__ubuntu_jammy_amd64/lastBuild/ + * ROS 2 iron jenkins build status: https://build.ros2.org/job/Idev__sick_scan_xd__ubuntu_jammy_amd64/lastBuild/ + * ROS 2 jazzy jenkins build status: https://build.ros2.org/job/Jdev__sick_scan_xd__ubuntu_noble_amd64/lastBuild/ + + +* Check apt version after 4-6 weeks with `sudo apt show ros--sick-scan-xd`: + ``` + cd /tmp + sudo apt update + sudo apt show ros-noetic-sick-scan-xd + sudo apt show ros-humble-sick-scan-xd + sudo apt show ros-iron-sick-scan-xd + sudo apt show ros-jazzy-sick-scan-xd + ``` + ## First time installation of toolchain 1. Install on Linux: @@ -322,17 +354,17 @@ Check `devel_branch` in https://github.com/ros2-gbp/sick_scan_xd-release/blob/ma **Bloom builds a new sick_scan_xd version, but apt still installs an old version** - * Check the sick_scan_xd version in the release repositories https://github.com/SICKAG/sick_scan_xd-release.git (ROS 1) and https://github.com/ros2-gbp/sick_scan_xd-release.git (ROS 2) - * Install bloom (if not yet done) using `sudo apt-get install python-bloom` on Linux or `pip install -U bloom` on Windows - * Run - ``` - bloom-release --rosdistro noetic -d sick_scan_xd # release repository: https://github.com/SICKAG/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro humble -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro iron -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - bloom-release --rosdistro jazzy -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos - ``` - * In case of github 2FA errors: Follow http://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization to create a 2FA token and configure the token in file `~/.config/bloom`. - * Note: Updates of release repository https://github.com/SICKAG/sick_scan_xd-release.git require github authentification via ssh. See https://docs.github.com/en/authentication/connecting-to-github-with-ssh and https://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization for details. +* Check the sick_scan_xd version in the release repositories https://github.com/SICKAG/sick_scan_xd-release.git (ROS 1) and https://github.com/ros2-gbp/sick_scan_xd-release.git (ROS 2) +* Install bloom (if not yet done) using `sudo apt-get install python-bloom` on Linux or `pip install -U bloom` on Windows +* Run `bloom-release` for ros noetic, humble, iron and jazzy: + ``` + bloom-release --rosdistro noetic -d sick_scan_xd # release repository: https://github.com/SICKAG/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro humble -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro iron -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + bloom-release --rosdistro jazzy -d sick_scan_xd # release repository: https://github.com/ros2-gbp/sick_scan_xd-release.git, argument -d enables debug infos + ``` +* In case of github 2FA errors: Follow http://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization to create a 2FA token and configure the token in file `~/.config/bloom`. +* Note: Updates of release repository https://github.com/SICKAG/sick_scan_xd-release.git require github authentification via ssh. See https://docs.github.com/en/authentication/connecting-to-github-with-ssh and https://wiki.ros.org/bloom/Tutorials/GithubManualAuthorization for details. # Testing diff --git a/driver/src/sick_scan_marker.cpp b/driver/src/sick_scan_marker.cpp index 23cf36e..a35c2f1 100644 --- a/driver/src/sick_scan_marker.cpp +++ b/driver/src/sick_scan_marker.cpp @@ -1,547 +1,550 @@ -/* - * @brief Implementation of object markers for sick_scan - * - * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim - * Copyright (C) 2021, SICK AG, Waldkirch - * All rights reserved. - * -* 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. -* -* -* 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 Osnabrueck University nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* * Neither the name of SICK AG nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission -* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. - * - * Created on: 13.01.2021 - * - * Authors: - * Michael Lehning - * - */ -#include -#include -#include - -#include "sick_scan/sick_scan_marker.h" - -#ifdef ROSSIMU -#include -#endif - -static ros_std_msgs::ColorRGBA color(float r, float g, float b, float a = 0.5f) -{ - ros_std_msgs::ColorRGBA color; - color.r = r; - color.g = g; - color.b = b; - color.a = a; - return color; -} - -static ros_std_msgs::ColorRGBA red(void) -{ - return color(1.0f, 0.0f, 0.0f); -} - -static ros_std_msgs::ColorRGBA green(void) // free fields -{ - return color(0.0f, 1.0f, 0.0f); -} - -static ros_std_msgs::ColorRGBA blue(void) -{ - return color(0.0f, 0.0f, 1.0f); -} - -static ros_std_msgs::ColorRGBA yellow(void) // infringed fields -{ - return color(1.0f, 1.0f, 0.0f); -} - -static ros_std_msgs::ColorRGBA gray(void) // invalid fields (default) -{ - return color(0.5f, 0.5f, 0.5f); -} - -sick_scan_xd::SickScanMarker::SickScanMarker(rosNodePtr nh, const std::string & marker_topic, const std::string & marker_frame_id) -: m_nh(nh), m_scan_mon_fieldset(0), m_marker_output_legend_offset_x(-0.5) -{ - if(nh) - { - m_frame_id = marker_frame_id.empty() ? "/cloud" : marker_frame_id; - m_marker_publisher = rosAdvertise(nh, marker_topic.empty() ? "sick_scan/marker" : marker_topic, 1); - m_add_transform_xyz_rpy = sick_scan_xd::SickCloudTransform(nh, true); - } -} - -sick_scan_xd::SickScanMarker::~SickScanMarker() -{ -} - -void sick_scan_xd::SickScanMarker::updateMarker(const std::vector& fields, int fieldset, int _eval_field_logic) -{ - sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; - m_scan_mon_fields = fields; - if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - m_scan_mon_fieldset = fieldset; - std::vector default_fields = {FieldInfo(0,0,"-","3",gray()), FieldInfo(1,0,"-","2",gray()), FieldInfo(2,0,"-","1",gray())}; - m_scan_mon_field_marker = createMonFieldMarker(default_fields); - m_scan_mon_field_legend = createMonFieldLegend(default_fields); - } - // if(eval_field_logic != USE_EVAL_FIELD_LMS5XX_LOGIC) - // m_scan_fieldset_legend = createMonFieldsetLegend(0); - // m_scan_outputstate_legend = createOutputStateLegend({"0", "0", "0"}, {"-", "-", "-"}, {gray(), gray(), gray()}); // only if outputstates active, i.e. after updateMarker(LIDoutputstateMsg) - publishMarker(); -} - -void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LIDinputstateMsg& msg, int _eval_field_logic) -{ - sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); - m_scan_mon_fieldset = fieldMon->getActiveFieldset(); - m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); - publishMarker(); - } -} - -void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LIDoutputstateMsg& msg, int _eval_field_logic) -{ - sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - m_scan_mon_fieldset = fieldMon->getActiveFieldset(); - ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); - } - int num_devices = (int)std::min(msg.output_count.size(), msg.output_state.size()); - std::vector output_state(num_devices); - std::vector output_count(num_devices); - std::vector output_colors(num_devices); - for(int field_idx = 0; field_idx < num_devices; field_idx++) - { - int count = msg.output_count[field_idx]; - int state = msg.output_state[field_idx]; - output_state[field_idx] = std::to_string(state); - output_count[field_idx] = std::to_string(count); - if(state == 1) // 1 = active = yellow - { - output_state[field_idx] = "[ON]"; - output_colors[field_idx] = yellow(); - } - else // 0 = not active = gray or 2 = not used = gray - { - output_state[field_idx] = "[OFF]"; - output_colors[field_idx] = gray(); - } - } - std::stringstream dbg_info; - dbg_info << "SickScanMarker::updateMarker(): LIDoutputstateMsg (state,count) = { "; - for(int field_idx = 0; field_idx < num_devices; field_idx++) - dbg_info << ((field_idx > 0) ? ", (" : "(") << output_state[field_idx] << "," << output_count[field_idx] << ")"; - dbg_info << " }"; - ROS_DEBUG_STREAM(dbg_info.str()); - if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); - m_scan_outputstate_legend = createOutputStateLegend(output_state, output_count, output_colors); - publishMarker(); -} - -void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LFErecMsg& msg, int _eval_field_logic) -{ - sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - m_scan_mon_fieldset = fieldMon->getActiveFieldset(); - ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); - } - std::vector field_info(msg.fields.size()); - for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++) - { - // LFErec: field_index runs from 1 to 3, field_info: field_index_scan_mon runs from 0 to 47 (field_index of m_scan_mon_fields) - field_info[field_idx].field_index_scan_mon = (int)(msg.fields[field_idx].field_index - 1 + msg.fields.size() * m_scan_mon_fieldset); - field_info[field_idx].field_result = msg.fields[field_idx].field_result_mrs; - if(field_info[field_idx].field_result == 1) // 1 = free/clear = green - { - field_info[field_idx].field_status = "Clear"; - field_info[field_idx].field_color = green(); - } - else if(field_info[field_idx].field_result == 2) // 2 = infringed = yellow - { - field_info[field_idx].field_status = "Infringed"; - field_info[field_idx].field_color = yellow(); - } - else // 0 = invalid = gray - { - field_info[field_idx].field_status = "Incorrect"; - field_info[field_idx].field_color = gray(); - } - if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - field_info[field_idx].field_name = std::to_string(field_info.size() - field_idx); // field_info[field_info_idx].field_index; - else - field_info[field_idx].field_name = std::to_string(msg.fields[field_idx].field_index); - } - std::stringstream dbg_info; - dbg_info << "SickScanMarker::updateMarker(): LFErec states={"; - for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++) - dbg_info << ((field_idx > 0) ? "," : "") << (int)msg.fields[field_idx].field_index << ":" << (int)msg.fields[field_idx].field_result_mrs; - dbg_info << "}, mon_field_point_cnt={"; - for(int field_idx = 0; field_idx < m_scan_mon_fields.size(); field_idx++) - dbg_info << ((field_idx > 0) ? "," : "") << m_scan_mon_fields[field_idx].getPointCount(); - dbg_info << "}, mon_field_set = " << m_scan_mon_fieldset; - ROS_DEBUG_STREAM(dbg_info.str()); - m_scan_mon_field_marker = createMonFieldMarker(field_info); - m_scan_mon_field_legend = createMonFieldLegend(field_info); - if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) - m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); - publishMarker(); -} - -void sick_scan_xd::SickScanMarker::publishMarker(void) -{ - ros_visualization_msgs::MarkerArray marker_array; - marker_array.markers.reserve(m_scan_mon_field_marker.size() + m_scan_mon_field_legend.size() + m_scan_outputstate_legend.size()); - for(int n = 0; n < m_scan_mon_field_marker.size(); n++) - marker_array.markers.push_back(m_scan_mon_field_marker[n]); - for(int n = 0; n < m_scan_mon_field_legend.size(); n++) - marker_array.markers.push_back(m_scan_mon_field_legend[n]); - for(int n = 0; n < m_scan_outputstate_legend.size(); n++) - marker_array.markers.push_back(m_scan_outputstate_legend[n]); - for(int n = 0; n < m_scan_fieldset_legend.size(); n++) - marker_array.markers.push_back(m_scan_fieldset_legend[n]); - notifyVisualizationMarkerListener(m_nh, &marker_array); - rosPublish(m_marker_publisher, marker_array); -#ifdef ROSSIMU - setVisualizationMarkerArray(marker_array.markers); // update ros simu output image -#endif -} - -static void appendTrianglePoints(int point_count, const std::vector& points_x, const std::vector& points_y, - ros_visualization_msgs::Marker& marker_point, int& triangle_idx, int nr_triangles, ros_std_msgs::ColorRGBA field_color) -{ - for(int point_idx = 2; point_idx < point_count && triangle_idx < nr_triangles; point_idx++, triangle_idx++) - { - - marker_point.points[3 * triangle_idx + 0].x = points_x[0]; - marker_point.points[3 * triangle_idx + 0].y = points_y[0]; - marker_point.points[3 * triangle_idx + 0].z = 0; - - marker_point.points[3 * triangle_idx + 1].x = points_x[point_idx - 1]; - marker_point.points[3 * triangle_idx + 1].y = points_y[point_idx - 1]; - marker_point.points[3 * triangle_idx + 1].z = 0; - - marker_point.points[3 * triangle_idx + 2].x = points_x[point_idx]; - marker_point.points[3 * triangle_idx + 2].y = points_y[point_idx]; - marker_point.points[3 * triangle_idx + 2].z = 0; - - marker_point.colors[3 * triangle_idx + 0] = field_color; - marker_point.colors[3 * triangle_idx + 1] = field_color; - marker_point.colors[3 * triangle_idx + 2] = field_color; - } -} - -std::vector sick_scan_xd::SickScanMarker::createMonFieldMarker(const std::vector& field_info) -{ - int nr_triangles = 0; - for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++) - { - int field_idx = field_info[field_info_idx].field_index_scan_mon; - const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; - SickScanMonFieldType field_typ = mon_field.fieldType(); - if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0) - nr_triangles += 2 * std::max(0, mon_field.getPointCount()/2 - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on - else - nr_triangles += std::max(0, mon_field.getPointCount() - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on - // std::map field_type_str = { {MON_FIELD_RADIAL, "MON_FIELD_RADIAL"}, {MON_FIELD_RECTANGLE, "MON_FIELD_RECTANGLE"}, {MON_FIELD_SEGMENTED, "MON_FIELD_SEGMENTED"}, {MON_FIELD_DYNAMIC, "MON_FIELD_DYNAMIC"} }; - // ROS_INFO_STREAM("sick_scan_xd::SickScanMarker::createMonFieldMarker(): field[" << field_info_idx << "]: type=" << field_type_str[field_typ] << ", " << (mon_field.getPointCount()) << " points"); - } - - // Draw fields using marker triangles - ros_visualization_msgs::Marker marker_point; - marker_point.header.stamp = rosTimeNow(); - marker_point.header.frame_id = m_frame_id; - marker_point.ns = "sick_scan"; - marker_point.id = 1; - marker_point.type = ros_visualization_msgs::Marker::TRIANGLE_LIST; - marker_point.scale.x = 1; - marker_point.scale.y = 1; - marker_point.scale.z = 1; - marker_point.pose.position.x = 0.0; - marker_point.pose.position.y = 0.0; - marker_point.pose.position.z = 0.0; - marker_point.pose.orientation.x = 0.0; - marker_point.pose.orientation.y = 0.0; - marker_point.pose.orientation.z = 0.0; - marker_point.pose.orientation.w = 1.0; - marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY - marker_point.color = gray(); - marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - - marker_point.points.resize(3 * nr_triangles); - marker_point.colors.resize(3 * nr_triangles); - std::vector triangle_centroids; - triangle_centroids.reserve(field_info.size()); - for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size() && triangle_idx < nr_triangles; field_info_idx++) - { - int field_idx = field_info[field_info_idx].field_index_scan_mon; - ros_std_msgs::ColorRGBA field_color = field_info[field_info_idx].field_color; - const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; - int point_count = mon_field.getPointCount(); - const std::vector& points_x = mon_field.getFieldPointsX(); - const std::vector& points_y = mon_field.getFieldPointsY(); - SickScanMonFieldType field_typ = mon_field.fieldType(); - if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0) - { - std::vector field1_points_x(point_count/2), field1_points_y(point_count/2), field2_points_x(point_count/2), field2_points_y(point_count/2); - for(int n = 0; n < point_count/2; n++) - { - field1_points_x[n] = points_x[n]; - field1_points_y[n] = points_y[n]; - field2_points_x[n] = points_x[n + point_count/2]; - field2_points_y[n] = points_y[n + point_count/2]; - } - ros_std_msgs::ColorRGBA field1_color = field_color, field2_color = field_color; - field1_color.r *= 0.5; - field1_color.g *= 0.5; - field1_color.b *= 0.5; - appendTrianglePoints(point_count/2, field1_points_x, field1_points_y, marker_point, triangle_idx, nr_triangles, field1_color); - appendTrianglePoints(point_count/2, field2_points_x, field2_points_y, marker_point, triangle_idx, nr_triangles, field2_color); - } - else - { - appendTrianglePoints(point_count, points_x, points_y, marker_point, triangle_idx, nr_triangles, field_color); - } - } - // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) - for(int n = 0; n < marker_point.points.size(); n++) - { - m_add_transform_xyz_rpy.applyTransform(marker_point.points[n].x, marker_point.points[n].y, marker_point.points[n].z); - } - - std::vector marker_array; - marker_array.reserve(1 + field_info.size()); - marker_array.push_back(marker_point); - - // Draw field names - for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++) - { - int field_idx = field_info[field_info_idx].field_index_scan_mon; - const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; - if(mon_field.getPointCount() >= 3) - { - ros_geometry_msgs::Point triangle_centroid; - triangle_centroid.x = 0; - triangle_centroid.y = 0; - triangle_centroid.z = 0; - const std::vector& points_x = mon_field.getFieldPointsX(); - const std::vector& points_y = mon_field.getFieldPointsY(); - for(int point_idx = 0; point_idx < mon_field.getPointCount(); point_idx++) - { - triangle_centroid.x += points_x[point_idx]; - triangle_centroid.y += points_y[point_idx]; - } - triangle_centroid.x /= (float)(mon_field.getPointCount()); - triangle_centroid.y /= (float)(mon_field.getPointCount()); - ros_visualization_msgs::Marker marker_field_name; - marker_field_name.header.stamp = rosTimeNow(); - marker_field_name.header.frame_id = m_frame_id; - marker_field_name.ns = "sick_scan"; - marker_field_name.id = 2 + field_info_idx; - marker_field_name.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; - marker_field_name.scale.z = 0.1; - marker_field_name.pose.position.x = triangle_centroid.x; - marker_field_name.pose.position.y = triangle_centroid.y; - marker_field_name.pose.position.z = triangle_centroid.z; - marker_field_name.pose.orientation.x = 0.0; - marker_field_name.pose.orientation.y = 0.0; - marker_field_name.pose.orientation.z = 0.0; - marker_field_name.pose.orientation.w = 1.0; - marker_field_name.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY - marker_field_name.color = field_info[field_info_idx].field_color; - marker_field_name.color.a = 1; - marker_field_name.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - marker_field_name.text = field_info[field_info_idx].field_name; - marker_array.push_back(marker_field_name); - } - else - { - ros_visualization_msgs::Marker marker_field_name; - marker_field_name.header.stamp = rosTimeNow(); - marker_field_name.header.frame_id = m_frame_id; - marker_field_name.ns = "sick_scan"; - marker_field_name.id = 2 + field_info_idx; -#if defined(_WIN32) && defined(DELETE) -# pragma push_macro("DELETE") -# undef DELETE - marker_field_name.action = ros_visualization_msgs::Marker::DELETE; -# pragma pop_macro("DELETE") -#else - marker_field_name.action = ros_visualization_msgs::Marker::DELETE; -#endif - marker_field_name.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - marker_array.push_back(marker_field_name); - } - - } - - return marker_array; -} - -std::vector sick_scan_xd::SickScanMarker::createMonFieldLegend(const std::vector& field_info) -{ - std::vector marker_array; - marker_array.reserve(2 * field_info.size()); - for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) - { - for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size(); field_info_idx++) - { - int field_idx = field_info[field_info_idx].field_index_scan_mon; - ros_visualization_msgs::Marker marker_point; - marker_point.header.stamp = rosTimeNow(); - marker_point.header.frame_id = m_frame_id; - marker_point.ns = "sick_scan"; - marker_point.id = 100 + loop_cnt * (int)m_scan_mon_fields.size() + field_info_idx; - marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; - marker_point.scale.z = 0.1; - marker_point.pose.position.x = -0.1 * field_info_idx - 0.1; - marker_point.pose.position.y = ((loop_cnt == 0) ? 0.3 : -0.2); - marker_point.pose.position.z = 0.0; - marker_point.pose.orientation.x = 0.0; - marker_point.pose.orientation.y = 0.0; - marker_point.pose.orientation.z = 0.0; - marker_point.pose.orientation.w = 1.0; - marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY - marker_point.color = field_info[field_info_idx].field_color; - marker_point.color.a = 1; - marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - std::stringstream marker_text; - // int detection_field_number = field_info.size() - field_info_idx; // field_info[field_info_idx].field_index; - if (loop_cnt == 0) - marker_text << "Detection field " << (field_info[field_info_idx].field_name) << " : "; - else - marker_text << field_info[field_info_idx].field_status; - marker_point.text = marker_text.str(); - marker_array.push_back(marker_point); - if(m_marker_output_legend_offset_x > marker_point.pose.position.x - 0.1) - m_marker_output_legend_offset_x = marker_point.pose.position.x - 0.1; - } - } - return marker_array; -} - -std::vector sick_scan_xd::SickScanMarker::createMonFieldsetLegend(int fieldset) -{ - std::vector marker_array; - marker_array.reserve(2); - for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) - { - ros_visualization_msgs::Marker marker_point; - marker_point.header.stamp = rosTimeNow(); - marker_point.header.frame_id = m_frame_id; - marker_point.ns = "sick_scan"; - marker_point.id = 500 + loop_cnt; - marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; - marker_point.scale.z = 0.1; - marker_point.pose.position.x = -0.4; - marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.2); - marker_point.pose.position.z = 0.0; - marker_point.pose.orientation.x = 0.0; - marker_point.pose.orientation.y = 0.0; - marker_point.pose.orientation.z = 0.0; - marker_point.pose.orientation.w = 1.0; - marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY - marker_point.color = green(); - marker_point.color.a = 1; - marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - std::stringstream marker_text; - if (loop_cnt == 0) - marker_text << "Fieldset :"; - else - marker_text << std::to_string(fieldset); - marker_point.text = marker_text.str(); - marker_array.push_back(marker_point); - } - return marker_array; -} - - -std::vector sick_scan_xd::SickScanMarker::createOutputStateLegend(const std::vector& output_state, const std::vector& output_count, const std::vector& output_colors) -{ - std::vector marker_array; - marker_array.reserve(2 * output_count.size()); - for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) - { - for(int field_idx = 0; field_idx < output_count.size(); field_idx++) - { - ros_visualization_msgs::Marker marker_point; - marker_point.header.stamp = rosTimeNow(); - marker_point.header.frame_id = m_frame_id; - marker_point.ns = "sick_scan"; - marker_point.id = 400 + loop_cnt * (int)output_count.size() + field_idx; - marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; - marker_point.scale.z = 0.1; - marker_point.pose.position.x = -0.1 * field_idx + m_marker_output_legend_offset_x; - marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.3); - marker_point.pose.position.z = 0.0; - marker_point.pose.orientation.x = 0.0; - marker_point.pose.orientation.y = 0.0; - marker_point.pose.orientation.z = 0.0; - marker_point.pose.orientation.w = 1.0; - marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY - marker_point.color = output_colors[field_idx]; - marker_point.color.a = 1; - marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever - std::stringstream marker_text; - int output_device = field_idx + 1; - if (loop_cnt == 0) - marker_text << "Output " << output_device << " : "; - else - marker_text << (field_idx < output_state.size() ? (output_state[field_idx]) : "") << " Count:" << output_count[field_idx]; - marker_point.text = marker_text.str(); - marker_array.push_back(marker_point); - } - } - return marker_array; -} +/* + * @brief Implementation of object markers for sick_scan + * + * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2021, SICK AG, Waldkirch + * All rights reserved. + * +* 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. +* +* +* 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 Osnabrueck University nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* * Neither the name of SICK AG nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission +* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. + * + * Created on: 13.01.2021 + * + * Authors: + * Michael Lehning + * + */ +#include +#include +#include + +#include "sick_scan/sick_scan_marker.h" + +#ifdef ROSSIMU +#include +#endif + +static ros_std_msgs::ColorRGBA color(float r, float g, float b, float a = 0.5f) +{ + ros_std_msgs::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + return color; +} + +static ros_std_msgs::ColorRGBA red(void) +{ + return color(1.0f, 0.0f, 0.0f); +} + +static ros_std_msgs::ColorRGBA green(void) // free fields +{ + return color(0.0f, 1.0f, 0.0f); +} + +static ros_std_msgs::ColorRGBA blue(void) +{ + return color(0.0f, 0.0f, 1.0f); +} + +static ros_std_msgs::ColorRGBA yellow(void) // infringed fields +{ + return color(1.0f, 1.0f, 0.0f); +} + +static ros_std_msgs::ColorRGBA gray(void) // invalid fields (default) +{ + return color(0.5f, 0.5f, 0.5f); +} + +sick_scan_xd::SickScanMarker::SickScanMarker(rosNodePtr nh, const std::string & marker_topic, const std::string & marker_frame_id) +: m_nh(nh), m_scan_mon_fieldset(1), m_marker_output_legend_offset_x(-0.5) +{ + if(nh) + { + m_frame_id = marker_frame_id.empty() ? "/cloud" : marker_frame_id; + m_marker_publisher = rosAdvertise(nh, marker_topic.empty() ? "sick_scan/marker" : marker_topic, 1); + m_add_transform_xyz_rpy = sick_scan_xd::SickCloudTransform(nh, true); + } +} + +sick_scan_xd::SickScanMarker::~SickScanMarker() +{ +} + +void sick_scan_xd::SickScanMarker::updateMarker(const std::vector& fields, int fieldset, int _eval_field_logic) +{ + sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; + m_scan_mon_fields = fields; + if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + m_scan_mon_fieldset = fieldset; + std::vector default_fields = {FieldInfo(0,0,"-","3",gray()), FieldInfo(1,0,"-","2",gray()), FieldInfo(2,0,"-","1",gray())}; + m_scan_mon_field_marker = createMonFieldMarker(default_fields); + m_scan_mon_field_legend = createMonFieldLegend(default_fields); + } + // if(eval_field_logic != USE_EVAL_FIELD_LMS5XX_LOGIC) + // m_scan_fieldset_legend = createMonFieldsetLegend(0); + // m_scan_outputstate_legend = createOutputStateLegend({"0", "0", "0"}, {"-", "-", "-"}, {gray(), gray(), gray()}); // only if outputstates active, i.e. after updateMarker(LIDoutputstateMsg) + publishMarker(); +} + +void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LIDinputstateMsg& msg, int _eval_field_logic) +{ + sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); + m_scan_mon_fieldset = fieldMon->getActiveFieldset(); + m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); + publishMarker(); + } +} + +void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LIDoutputstateMsg& msg, int _eval_field_logic) +{ + sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + m_scan_mon_fieldset = fieldMon->getActiveFieldset(); + ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); + } + int num_devices = (int)std::min(msg.output_count.size(), msg.output_state.size()); + std::vector output_state(num_devices); + std::vector output_count(num_devices); + std::vector output_colors(num_devices); + for(int field_idx = 0; field_idx < num_devices; field_idx++) + { + int count = msg.output_count[field_idx]; + int state = msg.output_state[field_idx]; + output_state[field_idx] = std::to_string(state); + output_count[field_idx] = std::to_string(count); + if(state == 1) // 1 = active = yellow + { + output_state[field_idx] = "[ON]"; + output_colors[field_idx] = yellow(); + } + else // 0 = not active = gray or 2 = not used = gray + { + output_state[field_idx] = "[OFF]"; + output_colors[field_idx] = gray(); + } + } + std::stringstream dbg_info; + dbg_info << "SickScanMarker::updateMarker(): LIDoutputstateMsg (state,count) = { "; + for(int field_idx = 0; field_idx < num_devices; field_idx++) + dbg_info << ((field_idx > 0) ? ", (" : "(") << output_state[field_idx] << "," << output_count[field_idx] << ")"; + dbg_info << " }"; + ROS_DEBUG_STREAM(dbg_info.str()); + if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); + m_scan_outputstate_legend = createOutputStateLegend(output_state, output_count, output_colors); + publishMarker(); +} + +void sick_scan_xd::SickScanMarker::updateMarker(sick_scan_msg::LFErecMsg& msg, int _eval_field_logic) +{ + sick_scan_xd::EVAL_FIELD_SUPPORT eval_field_logic = (sick_scan_xd::EVAL_FIELD_SUPPORT)_eval_field_logic; + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + if(fieldMon && eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + m_scan_mon_fieldset = fieldMon->getActiveFieldset(); + ROS_DEBUG_STREAM("SickScanMarker: active_fieldset = " << fieldMon->getActiveFieldset()); + } + std::vector field_info(msg.fields.size()); + for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++) + { + // LFErec: field_index runs from 1 to 3, field_info: field_index_scan_mon runs from 0 to 47 (field_index of m_scan_mon_fields) + assert(m_scan_mon_fieldset >= 1); // active fieldset assuming m_scan_mon_fieldset >= 1 (default: 1) + field_info[field_idx].field_index_scan_mon = (int)(msg.fields[field_idx].field_index - 1 + msg.fields.size() * (m_scan_mon_fieldset - 1)); + field_info[field_idx].field_result = msg.fields[field_idx].field_result_mrs; + if(field_info[field_idx].field_result == 1) // 1 = free/clear = green + { + field_info[field_idx].field_status = "Clear"; + field_info[field_idx].field_color = green(); + } + else if(field_info[field_idx].field_result == 2) // 2 = infringed = yellow + { + field_info[field_idx].field_status = "Infringed"; + field_info[field_idx].field_color = yellow(); + } + else // 0 = invalid = gray + { + field_info[field_idx].field_status = "Incorrect"; + field_info[field_idx].field_color = gray(); + } + if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + field_info[field_idx].field_name = std::to_string(field_info.size() - field_idx); // field_info[field_info_idx].field_index; + else + field_info[field_idx].field_name = std::to_string(msg.fields[field_idx].field_index); + } + std::stringstream dbg_info; + dbg_info << "SickScanMarker::updateMarker(): LFErec states={"; + for(int field_idx = 0; field_idx < msg.fields.size(); field_idx++) + dbg_info << ((field_idx > 0) ? "," : "") << (int)msg.fields[field_idx].field_index << ":" << (int)msg.fields[field_idx].field_result_mrs; + dbg_info << "}, mon_field_point_cnt={"; + for(int field_idx = 0; field_idx < m_scan_mon_fields.size(); field_idx++) + dbg_info << ((field_idx > 0) ? "," : "") << m_scan_mon_fields[field_idx].getPointCount(); + dbg_info << "}, mon_field_set = " << m_scan_mon_fieldset; + ROS_DEBUG_STREAM(dbg_info.str()); + m_scan_mon_field_marker = createMonFieldMarker(field_info); + m_scan_mon_field_legend = createMonFieldLegend(field_info); + if(eval_field_logic == USE_EVAL_FIELD_TIM7XX_LOGIC) + m_scan_fieldset_legend = createMonFieldsetLegend(m_scan_mon_fieldset); + publishMarker(); +} + +void sick_scan_xd::SickScanMarker::publishMarker(void) +{ + ros_visualization_msgs::MarkerArray marker_array; + marker_array.markers.reserve(m_scan_mon_field_marker.size() + m_scan_mon_field_legend.size() + m_scan_outputstate_legend.size()); + for(int n = 0; n < m_scan_mon_field_marker.size(); n++) + marker_array.markers.push_back(m_scan_mon_field_marker[n]); + for(int n = 0; n < m_scan_mon_field_legend.size(); n++) + marker_array.markers.push_back(m_scan_mon_field_legend[n]); + for(int n = 0; n < m_scan_outputstate_legend.size(); n++) + marker_array.markers.push_back(m_scan_outputstate_legend[n]); + for(int n = 0; n < m_scan_fieldset_legend.size(); n++) + marker_array.markers.push_back(m_scan_fieldset_legend[n]); + notifyVisualizationMarkerListener(m_nh, &marker_array); + rosPublish(m_marker_publisher, marker_array); +#ifdef ROSSIMU + setVisualizationMarkerArray(marker_array.markers); // update ros simu output image +#endif +} + +static void appendTrianglePoints(int point_count, const std::vector& points_x, const std::vector& points_y, + ros_visualization_msgs::Marker& marker_point, int& triangle_idx, int nr_triangles, ros_std_msgs::ColorRGBA field_color) +{ + for(int point_idx = 2; point_idx < point_count && triangle_idx < nr_triangles; point_idx++, triangle_idx++) + { + + marker_point.points[3 * triangle_idx + 0].x = points_x[0]; + marker_point.points[3 * triangle_idx + 0].y = points_y[0]; + marker_point.points[3 * triangle_idx + 0].z = 0; + + marker_point.points[3 * triangle_idx + 1].x = points_x[point_idx - 1]; + marker_point.points[3 * triangle_idx + 1].y = points_y[point_idx - 1]; + marker_point.points[3 * triangle_idx + 1].z = 0; + + marker_point.points[3 * triangle_idx + 2].x = points_x[point_idx]; + marker_point.points[3 * triangle_idx + 2].y = points_y[point_idx]; + marker_point.points[3 * triangle_idx + 2].z = 0; + + marker_point.colors[3 * triangle_idx + 0] = field_color; + marker_point.colors[3 * triangle_idx + 1] = field_color; + marker_point.colors[3 * triangle_idx + 2] = field_color; + } +} + +std::vector sick_scan_xd::SickScanMarker::createMonFieldMarker(const std::vector& field_info) +{ + int nr_triangles = 0; + for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++) + { + int field_idx = field_info[field_info_idx].field_index_scan_mon; + assert(field_idx >= 0 && field_idx < m_scan_mon_fields.size()); + const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; + SickScanMonFieldType field_typ = mon_field.fieldType(); + if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0) + nr_triangles += 2 * std::max(0, mon_field.getPointCount()/2 - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on + else + nr_triangles += std::max(0, mon_field.getPointCount() - 2); // 3 points: 1 triangle, 4 points: 3 triangles, and so on + // std::map field_type_str = { {MON_FIELD_RADIAL, "MON_FIELD_RADIAL"}, {MON_FIELD_RECTANGLE, "MON_FIELD_RECTANGLE"}, {MON_FIELD_SEGMENTED, "MON_FIELD_SEGMENTED"}, {MON_FIELD_DYNAMIC, "MON_FIELD_DYNAMIC"} }; + // ROS_INFO_STREAM("sick_scan_xd::SickScanMarker::createMonFieldMarker(): field[" << field_info_idx << "]: type=" << field_type_str[field_typ] << ", " << (mon_field.getPointCount()) << " points"); + } + + // Draw fields using marker triangles + ros_visualization_msgs::Marker marker_point; + marker_point.header.stamp = rosTimeNow(); + marker_point.header.frame_id = m_frame_id; + marker_point.ns = "sick_scan"; + marker_point.id = 1; + marker_point.type = ros_visualization_msgs::Marker::TRIANGLE_LIST; + marker_point.scale.x = 1; + marker_point.scale.y = 1; + marker_point.scale.z = 1; + marker_point.pose.position.x = 0.0; + marker_point.pose.position.y = 0.0; + marker_point.pose.position.z = 0.0; + marker_point.pose.orientation.x = 0.0; + marker_point.pose.orientation.y = 0.0; + marker_point.pose.orientation.z = 0.0; + marker_point.pose.orientation.w = 1.0; + marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY + marker_point.color = gray(); + marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + + marker_point.points.resize(3 * nr_triangles); + marker_point.colors.resize(3 * nr_triangles); + std::vector triangle_centroids; + triangle_centroids.reserve(field_info.size()); + for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size() && triangle_idx < nr_triangles; field_info_idx++) + { + int field_idx = field_info[field_info_idx].field_index_scan_mon; + assert(field_idx >= 0 && field_idx < m_scan_mon_fields.size()); + ros_std_msgs::ColorRGBA field_color = field_info[field_info_idx].field_color; + const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; + int point_count = mon_field.getPointCount(); + const std::vector& points_x = mon_field.getFieldPointsX(); + const std::vector& points_y = mon_field.getFieldPointsY(); + SickScanMonFieldType field_typ = mon_field.fieldType(); + if(field_typ == MON_FIELD_DYNAMIC) // dynamic fields have two rectangle (first rectangle for v = max, second rectangle for v = 0) + { + std::vector field1_points_x(point_count/2), field1_points_y(point_count/2), field2_points_x(point_count/2), field2_points_y(point_count/2); + for(int n = 0; n < point_count/2; n++) + { + field1_points_x[n] = points_x[n]; + field1_points_y[n] = points_y[n]; + field2_points_x[n] = points_x[n + point_count/2]; + field2_points_y[n] = points_y[n + point_count/2]; + } + ros_std_msgs::ColorRGBA field1_color = field_color, field2_color = field_color; + field1_color.r *= 0.5; + field1_color.g *= 0.5; + field1_color.b *= 0.5; + appendTrianglePoints(point_count/2, field1_points_x, field1_points_y, marker_point, triangle_idx, nr_triangles, field1_color); + appendTrianglePoints(point_count/2, field2_points_x, field2_points_y, marker_point, triangle_idx, nr_triangles, field2_color); + } + else + { + appendTrianglePoints(point_count, points_x, points_y, marker_point, triangle_idx, nr_triangles, field_color); + } + } + // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) + for(int n = 0; n < marker_point.points.size(); n++) + { + m_add_transform_xyz_rpy.applyTransform(marker_point.points[n].x, marker_point.points[n].y, marker_point.points[n].z); + } + + std::vector marker_array; + marker_array.reserve(1 + field_info.size()); + marker_array.push_back(marker_point); + + // Draw field names + for(int field_info_idx = 0; field_info_idx < field_info.size(); field_info_idx++) + { + int field_idx = field_info[field_info_idx].field_index_scan_mon; + assert(field_idx >= 0 && field_idx < m_scan_mon_fields.size()); + const sick_scan_xd::SickScanMonField& mon_field = m_scan_mon_fields[field_idx]; + if(mon_field.getPointCount() >= 3) + { + ros_geometry_msgs::Point triangle_centroid; + triangle_centroid.x = 0; + triangle_centroid.y = 0; + triangle_centroid.z = 0; + const std::vector& points_x = mon_field.getFieldPointsX(); + const std::vector& points_y = mon_field.getFieldPointsY(); + for(int point_idx = 0; point_idx < mon_field.getPointCount(); point_idx++) + { + triangle_centroid.x += points_x[point_idx]; + triangle_centroid.y += points_y[point_idx]; + } + triangle_centroid.x /= (float)(mon_field.getPointCount()); + triangle_centroid.y /= (float)(mon_field.getPointCount()); + ros_visualization_msgs::Marker marker_field_name; + marker_field_name.header.stamp = rosTimeNow(); + marker_field_name.header.frame_id = m_frame_id; + marker_field_name.ns = "sick_scan"; + marker_field_name.id = 2 + field_info_idx; + marker_field_name.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; + marker_field_name.scale.z = 0.1; + marker_field_name.pose.position.x = triangle_centroid.x; + marker_field_name.pose.position.y = triangle_centroid.y; + marker_field_name.pose.position.z = triangle_centroid.z; + marker_field_name.pose.orientation.x = 0.0; + marker_field_name.pose.orientation.y = 0.0; + marker_field_name.pose.orientation.z = 0.0; + marker_field_name.pose.orientation.w = 1.0; + marker_field_name.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY + marker_field_name.color = field_info[field_info_idx].field_color; + marker_field_name.color.a = 1; + marker_field_name.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + marker_field_name.text = field_info[field_info_idx].field_name; + marker_array.push_back(marker_field_name); + } + else + { + ros_visualization_msgs::Marker marker_field_name; + marker_field_name.header.stamp = rosTimeNow(); + marker_field_name.header.frame_id = m_frame_id; + marker_field_name.ns = "sick_scan"; + marker_field_name.id = 2 + field_info_idx; +#if defined(_WIN32) && defined(DELETE) +# pragma push_macro("DELETE") +# undef DELETE + marker_field_name.action = ros_visualization_msgs::Marker::DELETE; +# pragma pop_macro("DELETE") +#else + marker_field_name.action = ros_visualization_msgs::Marker::DELETE; +#endif + marker_field_name.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + marker_array.push_back(marker_field_name); + } + + } + + return marker_array; +} + +std::vector sick_scan_xd::SickScanMarker::createMonFieldLegend(const std::vector& field_info) +{ + std::vector marker_array; + marker_array.reserve(2 * field_info.size()); + for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) + { + for(int field_info_idx = 0, triangle_idx = 0; field_info_idx < field_info.size(); field_info_idx++) + { + ros_visualization_msgs::Marker marker_point; + marker_point.header.stamp = rosTimeNow(); + marker_point.header.frame_id = m_frame_id; + marker_point.ns = "sick_scan"; + marker_point.id = 100 + loop_cnt * (int)m_scan_mon_fields.size() + field_info_idx; + marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; + marker_point.scale.z = 0.1; + marker_point.pose.position.x = -0.1 * field_info_idx - 0.1; + marker_point.pose.position.y = ((loop_cnt == 0) ? 0.3 : -0.2); + marker_point.pose.position.z = 0.0; + marker_point.pose.orientation.x = 0.0; + marker_point.pose.orientation.y = 0.0; + marker_point.pose.orientation.z = 0.0; + marker_point.pose.orientation.w = 1.0; + marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY + marker_point.color = field_info[field_info_idx].field_color; + marker_point.color.a = 1; + marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + std::stringstream marker_text; + // int detection_field_number = field_info.size() - field_info_idx; // field_info[field_info_idx].field_index; + if (loop_cnt == 0) + marker_text << "Detection field " << (field_info[field_info_idx].field_name) << " : "; + else + marker_text << field_info[field_info_idx].field_status; + marker_point.text = marker_text.str(); + marker_array.push_back(marker_point); + if(m_marker_output_legend_offset_x > marker_point.pose.position.x - 0.1) + m_marker_output_legend_offset_x = marker_point.pose.position.x - 0.1; + } + } + return marker_array; +} + +std::vector sick_scan_xd::SickScanMarker::createMonFieldsetLegend(int fieldset) +{ + std::vector marker_array; + marker_array.reserve(2); + for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) + { + ros_visualization_msgs::Marker marker_point; + marker_point.header.stamp = rosTimeNow(); + marker_point.header.frame_id = m_frame_id; + marker_point.ns = "sick_scan"; + marker_point.id = 500 + loop_cnt; + marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; + marker_point.scale.z = 0.1; + marker_point.pose.position.x = -0.4; + marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.2); + marker_point.pose.position.z = 0.0; + marker_point.pose.orientation.x = 0.0; + marker_point.pose.orientation.y = 0.0; + marker_point.pose.orientation.z = 0.0; + marker_point.pose.orientation.w = 1.0; + marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY + marker_point.color = green(); + marker_point.color.a = 1; + marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + std::stringstream marker_text; + if (loop_cnt == 0) + marker_text << "Fieldset :"; + else + marker_text << std::to_string(fieldset); + marker_point.text = marker_text.str(); + marker_array.push_back(marker_point); + } + return marker_array; +} + + +std::vector sick_scan_xd::SickScanMarker::createOutputStateLegend(const std::vector& output_state, const std::vector& output_count, const std::vector& output_colors) +{ + std::vector marker_array; + marker_array.reserve(2 * output_count.size()); + for(int loop_cnt = 0; loop_cnt < 2; loop_cnt++) + { + for(int field_idx = 0; field_idx < output_count.size(); field_idx++) + { + ros_visualization_msgs::Marker marker_point; + marker_point.header.stamp = rosTimeNow(); + marker_point.header.frame_id = m_frame_id; + marker_point.ns = "sick_scan"; + marker_point.id = 400 + loop_cnt * (int)output_count.size() + field_idx; + marker_point.type = ros_visualization_msgs::Marker::TEXT_VIEW_FACING; + marker_point.scale.z = 0.1; + marker_point.pose.position.x = -0.1 * field_idx + m_marker_output_legend_offset_x; + marker_point.pose.position.y = ((loop_cnt == 0) ? 0.16 : -0.3); + marker_point.pose.position.z = 0.0; + marker_point.pose.orientation.x = 0.0; + marker_point.pose.orientation.y = 0.0; + marker_point.pose.orientation.z = 0.0; + marker_point.pose.orientation.w = 1.0; + marker_point.action = ros_visualization_msgs::Marker::ADD; // note: ADD == MODIFY + marker_point.color = output_colors[field_idx]; + marker_point.color.a = 1; + marker_point.lifetime = rosDurationFromSec(0); // lifetime 0 indicates forever + std::stringstream marker_text; + int output_device = field_idx + 1; + if (loop_cnt == 0) + marker_text << "Output " << output_device << " : "; + else + marker_text << (field_idx < output_state.size() ? (output_state[field_idx]) : "") << " Count:" << output_count[field_idx]; + marker_point.text = marker_text.str(); + marker_array.push_back(marker_point); + } + } + return marker_array; +} diff --git a/driver/src/sick_scan_services.cpp b/driver/src/sick_scan_services.cpp index e328bd6..3936898 100644 --- a/driver/src/sick_scan_services.cpp +++ b/driver/src/sick_scan_services.cpp @@ -1,1171 +1,1180 @@ -/* - * @brief Implementation of ROS services for sick_scan - * - * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim - * Copyright (C) 2021, SICK AG, Waldkirch - * All rights reserved. - * -* 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. -* -* -* 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 Osnabrueck University nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* * Neither the name of SICK AG nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission -* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. - * - * Created on: 12.01.2021 - * - * Authors: - * Michael Lehning - * - * Based on the TiM communication example by SICK AG. - * - */ -#include -#include - -#include "sick_scan/sick_scan_services.h" -#include "sick_scan/sick_generic_laser.h" -#include "sick_scansegment_xd/udp_poster.h" - -#define SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN (true) // Arguments of SOPAS commands are big endian encoded - -sick_scan_xd::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan_xd::SickScanCommonTcp* common_tcp, ScannerBasicParam * lidar_param) -: m_common_tcp(common_tcp), m_cola_binary(true) -{ - bool srvSupportColaMsg = true, srvSupportECRChangeArr = true, srvSupportLIDoutputstate = true, srvSupportSCdevicestate = true; - bool srvSupportSCreboot = true, srvSupportSCsoftreset = true, srvSupportSickScanExit = true; - bool srvSupportGetContaminationResult = false; - if(lidar_param) - { - m_cola_binary = lidar_param->getUseBinaryProtocol(); - if(lidar_param->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) - { - srvSupportECRChangeArr = false; - srvSupportLIDoutputstate = false; - srvSupportSCreboot = false; - srvSupportSCsoftreset = false; - } - if(lidar_param->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0 - || lidar_param->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0 - || lidar_param->getScannerName().compare(SICK_SCANNER_SCANSEGMENT_XD_NAME) == 0) - { - srvSupportGetContaminationResult = true; // "sRN ContaminationResult" supported by MRS-1000, LMS-1000, multiScan - } - } - if(nh) - { - m_client_authorization_pw = "F4724744"; - if (lidar_param->getUseSafetyPasWD()) // TIM_7xxS - 1 layer Safety Scanner - m_client_authorization_pw = "6FD62C05"; - rosDeclareParam(nh, "client_authorization_pw", m_client_authorization_pw); - rosGetParam(nh, "client_authorization_pw", m_client_authorization_pw); - -#if __ROS_VERSION == 2 -#define serviceCbColaMsgROS sick_scan_xd::SickScanServices::serviceCbColaMsgROS2 -#define serviceCbECRChangeArrROS sick_scan_xd::SickScanServices::serviceCbECRChangeArrROS2 -#define serviceCbLIDoutputstateROS sick_scan_xd::SickScanServices::serviceCbLIDoutputstateROS2 -#define serviceCbSCdevicestateROS sick_scan_xd::SickScanServices::serviceCbSCdevicestateROS2 -#define serviceCbSCrebootROS sick_scan_xd::SickScanServices::serviceCbSCrebootROS2 -#define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftresetROS2 -#define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExitROS2 -#define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResultROS2 -#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetReadROS2 -#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWriteROS2 -#else -#define serviceCbColaMsgROS sick_scan_xd::SickScanServices::serviceCbColaMsg -#define serviceCbECRChangeArrROS sick_scan_xd::SickScanServices::serviceCbECRChangeArr -#define serviceCbLIDoutputstateROS sick_scan_xd::SickScanServices::serviceCbLIDoutputstate -#define serviceCbSCdevicestateROS sick_scan_xd::SickScanServices::serviceCbSCdevicestate -#define serviceCbSCrebootROS sick_scan_xd::SickScanServices::serviceCbSCreboot -#define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftreset -#define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExit -#define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResult -#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetRead -#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWrite -#endif -#if __ROS_VERSION == 1 -#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a.getService() << "\" created (\"" << b.getService() << "\")"); -#elif __ROS_VERSION == 2 -#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a->get_service_name() << "\" created (\"" << b->get_service_name() << "\")"); -#else -#define printServiceCreated(a,b) -#endif - if(srvSupportColaMsg) - { - auto srv_server_ColaMsg = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ColaMsgSrv, "ColaMsg", &serviceCbColaMsgROS, this); - m_srv_server_ColaMsg = rosServiceServer(srv_server_ColaMsg); - printServiceCreated(srv_server_ColaMsg, m_srv_server_ColaMsg); - } - if(srvSupportECRChangeArr) - { - auto srv_server_ECRChangeArr = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ECRChangeArrSrv, "ECRChangeArr", &serviceCbECRChangeArrROS, this); - m_srv_server_ECRChangeArr = rosServiceServer(srv_server_ECRChangeArr); - printServiceCreated(srv_server_ECRChangeArr, m_srv_server_ECRChangeArr); - } - if(srvSupportGetContaminationResult) - { - auto srv_server_GetContaminationResult = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::GetContaminationResultSrv, "GetContaminationResult", &serviceCbGetContaminationResultROS, this); - m_srv_server_GetContaminationResult = rosServiceServer(srv_server_GetContaminationResult); - printServiceCreated(srv_server_GetContaminationResult, m_srv_server_GetContaminationResult); - } - if(srvSupportLIDoutputstate) - { - auto srv_server_LIDoutputstate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::LIDoutputstateSrv, "LIDoutputstate", &serviceCbLIDoutputstateROS, this); - m_srv_server_LIDoutputstate = rosServiceServer(srv_server_LIDoutputstate); - printServiceCreated(srv_server_LIDoutputstate, m_srv_server_LIDoutputstate); - } - if(srvSupportSCdevicestate) - { - auto srv_server_SCdevicestate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCdevicestateSrv, "SCdevicestate", &serviceCbSCdevicestateROS, this); - m_srv_server_SCdevicestate = rosServiceServer(srv_server_SCdevicestate); - printServiceCreated(srv_server_SCdevicestate, m_srv_server_SCdevicestate); - } - if(srvSupportSCreboot) - { - auto srv_server_SCreboot = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCrebootSrv, "SCreboot", &serviceCbSCrebootROS, this); - m_srv_server_SCreboot = rosServiceServer(srv_server_SCreboot); - printServiceCreated(srv_server_SCreboot, m_srv_server_SCreboot); - } - if(srvSupportSCsoftreset) - { - auto srv_server_SCsoftreset = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCsoftresetSrv, "SCsoftreset", &serviceCbSCsoftresetROS, this); - m_srv_server_SCsoftreset = rosServiceServer(srv_server_SCsoftreset); - printServiceCreated(srv_server_SCsoftreset, m_srv_server_SCsoftreset); - } - if(srvSupportSickScanExit) - { - auto srv_server_SickScanExit = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SickScanExitSrv, "SickScanExit", &serviceCbSickScanExitROS, this); - m_srv_server_SickScanExit = rosServiceServer(srv_server_SickScanExit); - printServiceCreated(srv_server_SickScanExit, m_srv_server_SickScanExit); - } -#if __ROS_VERSION > 0 - if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - auto srv_server_FieldSetRead = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetReadSrv, "FieldSetRead", &serviceCbFieldSetReadROS, this); - m_srv_server_FieldSetRead = rosServiceServer(srv_server_FieldSetRead); - printServiceCreated(srv_server_FieldSetRead, m_srv_server_FieldSetRead); - } - if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) - { - auto srv_server_FieldSetWrite = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetWriteSrv, "FieldSetWrite", &serviceCbFieldSetWriteROS, this); - m_srv_server_FieldSetWrite = rosServiceServer(srv_server_FieldSetWrite); - printServiceCreated(srv_server_FieldSetWrite, m_srv_server_FieldSetWrite); - } -#endif - } -} - -sick_scan_xd::SickScanServices::~SickScanServices() -{ -} - -/*! - * Sends a sopas command and returns the lidar reply. - * @param[in] sopasCmd sopas command to send, f.e. "sEN ECRChangeArr 1" - * @param[out] sopasReplyBin response from lidar incl. start/stop byte - * @param[out] sopasReplyString sopasReplyBin converted to string - * @return true on success, false in case of errors. - */ -bool sick_scan_xd::SickScanServices::sendSopasAndCheckAnswer(const std::string& sopasCmd, std::vector& sopasReplyBin, std::string& sopasReplyString) -{ - if(m_common_tcp) - { - ROS_INFO_STREAM("SickScanServices: Sending request \"" << sopasCmd << "\""); - std::string sopasRequest = std::string("\x02") + sopasCmd + "\x03"; - int result = -1; - if (m_cola_binary) - { - std::vector reqBinary; - m_common_tcp->convertAscii2BinaryCmd(sopasRequest.c_str(), &reqBinary); - result = m_common_tcp->sendSopasAndCheckAnswer(reqBinary, &sopasReplyBin, -1); - } - else - { - result = m_common_tcp->sendSopasAndCheckAnswer(sopasRequest.c_str(), &sopasReplyBin, -1); - } - if (result != 0) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command \"" << sopasCmd << "\""); - } - else - { - sopasReplyString = m_common_tcp->sopasReplyToString(sopasReplyBin); - ROS_INFO_STREAM("SickScanServices: Request \"" << sopasCmd << "\" successfully sent, received reply \"" << sopasReplyString << "\""); - return true; - } - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: m_common_tcp not initialized"); - } - return false; -} - -/*! - * Callback for service ColaMsg (ColaMsg, send a cola message to lidar). - * @param[in] service_request ros service request to lidar - * @param[out] service_response service response from lidar - * @return true on success, false in case of errors. - */ -bool sick_scan_xd::SickScanServices::serviceCbColaMsg(sick_scan_srv::ColaMsgSrv::Request &service_request, sick_scan_srv::ColaMsgSrv::Response &service_response) -{ - std::string sopasCmd = service_request.request; - std::vector sopasReplyBin; - std::string sopasReplyString; - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - service_response.response = sopasReplyString; - return true; -} - -/*! - * Callback for service messages (ECRChangeArr, Request status change of monitoring fields on event). - * Sends a cola telegram "sEN ECRChangeArr {0|1}" and receives the response from the lidar device. - * @param[in] service_request ros service request to lidar - * @param[out] service_response service response from lidar - * @return true on success, false in case of errors. - */ -bool sick_scan_xd::SickScanServices::serviceCbECRChangeArr(sick_scan_srv::ECRChangeArrSrv::Request &service_request, sick_scan_srv::ECRChangeArrSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sEN ECRChangeArr ") + (service_request.active ? "1" : "0"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - return true; -} - -/*! -* Callbacks for service messages. -* @param[in] service_request ros service request to lidar -* @param[out] service_response service response from lidar -* @return true on success, false in case of errors. -*/ -bool sick_scan_xd::SickScanServices::serviceCbGetContaminationResult(sick_scan_srv::GetContaminationResultSrv::Request &service_request, sick_scan_srv::GetContaminationResultSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sRN ContaminationResult"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - service_response.warning = 0; - service_response.error = 0; - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - std::string response_str((char*)sopasReplyBin.data(), sopasReplyBin.size()); - std::size_t state_pos = response_str.find("ContaminationResult"); - int result_idx = 20; - if (state_pos != std::string::npos && state_pos + result_idx < sopasReplyBin.size()) - { - uint8_t result_byte = sopasReplyBin[state_pos + result_idx]; - result_byte = ((result_byte >= '0') ? (result_byte - '0') : (result_byte)); // convert to bin in case of ascii - service_response.warning = result_byte; - result_idx++; - if (result_idx < sopasReplyBin.size() && sopasReplyBin[state_pos + result_idx] == ' ') // jump over ascii separator - result_idx++; - if (result_idx < sopasReplyBin.size()) - { - result_byte = sopasReplyBin[state_pos + result_idx]; - result_byte = ((result_byte >= '0') ? (result_byte - '0') : (result_byte)); // convert to bin in case of ascii - service_response.error = result_byte; - } - } - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\" = \"" << DataDumper::binDataToAsciiString(sopasReplyBin.data(), sopasReplyBin.size()) << "\"" - << " (response.success=" << (int)(service_response.success) << ", response.warning=" << (int)(service_response.warning) << ", response.error=" << (int)(service_response.error) << ")"); - - return true; -} - - -/*! - * Callback for service messages (LIDoutputstate, Request status change of monitoring fields on event). - * Sends a cola telegram "sEN LIDoutputstate {0|1}" and receives the response from the lidar device. - * @param[in] service_request ros service request to lidar - * @param[out] service_response service response from lidar - * @return true on success, false in case of errors. - */ -bool sick_scan_xd::SickScanServices::serviceCbLIDoutputstate(sick_scan_srv::LIDoutputstateSrv::Request &service_request, sick_scan_srv::LIDoutputstateSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sEN LIDoutputstate ") + (service_request.active ? "1" : "0"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - return true; -} - -/*! - * Sends the SOPAS authorization command "sMN SetAccessMode 3 F4724744". - */ -bool sick_scan_xd::SickScanServices::sendAuthorization() -{ - std::string sopasCmd = std::string("sMN SetAccessMode 3 ") + m_client_authorization_pw; - std::vector sopasReplyBin; - std::string sopasReplyString; - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - return true; -} - -/*! - * Sends the SOPAS command "sMN Run", which applies previous send settings - */ -bool sick_scan_xd::SickScanServices::sendRun() -{ - std::string sopasCmd = std::string("sMN Run"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - return true; -} - -/*! - * Sends a multiScan136 command - */ -bool sick_scan_xd::SickScanServices::sendSopasCmdCheckResponse(const std::string& sopas_request, const std::string& expected_response) -{ - std::vector sopasReplyBin; - std::string sopasReplyString; - if(!sendSopasAndCheckAnswer(sopas_request, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse() failed on sending command\"" << sopas_request << "\""); - return false; - } - ROS_INFO_STREAM("SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", response: \"" << sopasReplyString << "\""); - if(sopasReplyString.find(expected_response) == std::string::npos) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", unexpected response: \"" << sopasReplyString << "\", \"" << expected_response << "\" not found"); - return false; - } - return true; -} - -#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 -static void printPicoScanImuDisabledWarning() -{ - ROS_WARN_STREAM("############################"); - ROS_WARN_STREAM("## ##"); - ROS_WARN_STREAM("## IMU will be disabled ##"); - ROS_WARN_STREAM("## ##"); - ROS_WARN_STREAM("############################"); - ROS_WARN_STREAM("## If you are using a picoScan150 w/o addons, disable the IMU by setting option \"imu_enable\" to \"False\" in your launchfile, or use sick_picoscan_single_echo.launch to avoid this error"); - ROS_WARN_STREAM("## If you are using a picoScan with IMU, check IMU settings with SOPAS Air"); -} - -/*! -* Sends the multiScan start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" -* @param[in] hostname IP address of multiScan136, default 192.168.0.1 -* @param[in] port IP port of multiScan136, default 2115 -* @param[in] scanner_type type of scanner, currently supported are multiScan136 and picoScan150 -* @param[in] scandataformat ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 2 -* @param[in+out] imu_enable: Imu data transfer enabled -* @param[in] imu_udp_port: UDP port of imu data (if imu_enable is true) -* @param[in] check_udp_receiver_ip: check udp_receiver_ip by sending and receiving a udp test message -* @param[in] check_udp_receiver_port: udp port to check udp_receiver_ip -*/ -bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool& imu_enable, int imu_udp_port, int performanceprofilenumber, bool check_udp_receiver_ip, int check_udp_receiver_port) -{ - // Check udp receiver ip address: hostname is expected to be a IPv4 address - std::stringstream ip_stream(hostname); - std::string ip_token; - std::vector ip_tokens; - while (getline(ip_stream, ip_token, '.')) - { - ip_tokens.push_back(ip_token); - } - if (ip_tokens.size() != 4) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed: can't split ip address \"" << hostname << "\" into 4 tokens, check ip address."); - ROS_ERROR_STREAM("## ERROR parsing ip address, check configuration of parameter udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); - ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); - return false; - } - // Check udp receiver ip address by sending and receiving a UDP test message - std::string check_udp_receiver_msg = "sick_scan_xd udp test message verifying udp_receiver_ip"; - if (check_udp_receiver_ip && !check_udp_receiver_msg.empty()) - { - sick_scansegment_xd::UdpPoster udp_loopback_poster(hostname, check_udp_receiver_port); - std::string check_udp_receiver_response; - if (!udp_loopback_poster.Post(check_udp_receiver_msg, check_udp_receiver_response) || check_udp_receiver_msg != check_udp_receiver_response) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed to send and receive a UDP test messsage to " << hostname << ":" << check_udp_receiver_port); - ROS_ERROR_STREAM("## Check configuration of parameter udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); - ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); - return false; - } - else - { - ROS_DEBUG_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message \"" << check_udp_receiver_response << "\" successfully received."); - ROS_INFO_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message successfully send to " << hostname << ":" << check_udp_receiver_port << ", parameter udp_receiver_ip=\"" << hostname << "\" is valid."); - } - } - // Send sopas start sequence (ScanDataFormat, ScanDataEthSettings, ImuDataEthSettings, ScanDataEnable, ImuDataEnable) - std::stringstream eth_settings_cmd, imu_eth_settings_cmd, scandataformat_cmd, performanceprofilenumber_cmd; - scandataformat_cmd << "sWN ScanDataFormat " << scandataformat; - if (performanceprofilenumber >= 0) - { - performanceprofilenumber_cmd << "sWN PerformanceProfileNumber " << std::uppercase << std::hex << performanceprofilenumber; - } - eth_settings_cmd << "sWN ScanDataEthSettings 1"; - imu_eth_settings_cmd << "sWN ImuDataEthSettings 1"; - for (int i = 0; i < ip_tokens.size(); i++) - { - eth_settings_cmd << " +"; - eth_settings_cmd << ip_tokens[i]; - imu_eth_settings_cmd << " +"; - imu_eth_settings_cmd << ip_tokens[i]; - } - eth_settings_cmd << " +"; - eth_settings_cmd << port; - imu_eth_settings_cmd << " +"; - imu_eth_settings_cmd << imu_udp_port; - if (!sendSopasCmdCheckResponse(eth_settings_cmd.str(), "sWA ScanDataEthSettings")) // configure destination scan data output destination , f.e. "sWN ScanDataEthSettings 1 +192 +168 +0 +52 +2115" (ip 192.168.0.52 port 2115) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEthSettings 1\") failed."); - return false; - } - if (scandataformat != 1 && scandataformat != 2) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): invalid scandataformat configuration, unsupported scandataformat=" << scandataformat << ", check configuration and use 1 for msgpack or 2 for compact data"); - return false; - } - if (!sendSopasCmdCheckResponse(scandataformat_cmd.str(), "sWA ScanDataFormat")) // set scan data output format to MSGPACK (1) or COMPACT (2) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataFormat 1\") failed."); - return false; - } - if (performanceprofilenumber >= 0) - { - if (!sendSopasCmdCheckResponse(performanceprofilenumber_cmd.str(), "sWA PerformanceProfileNumber")) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN PerformanceProfileNumber ..\") failed."); - return false; - } - } - if (scanner_type == SICK_SCANNER_SCANSEGMENT_XD_NAME && !sendSopasCmdCheckResponse("sWN ScanDataPreformatting 1", "sWA ScanDataPreformatting")) // ScanDataPreformatting for multiScan136 only - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataPreformatting 1\") failed."); - return false; - } - if (imu_enable && !sendSopasCmdCheckResponse(imu_eth_settings_cmd.str(), "sWA ImuDataEthSettings")) // imu data eth settings - { - if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) - { - // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). - // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. - ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); - printPicoScanImuDisabledWarning(); - imu_enable = false; - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); - } - } - if (!sendRun()) - { - return false; - } - if (!sendAuthorization()) - { - return false; - } - if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 1", "sWA ScanDataEnable")) // enable scan data output - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 1\") failed."); - return false; - } - if (imu_enable && !sendSopasCmdCheckResponse("sWN ImuDataEnable 1", "sWA ImuDataEnable")) // enable imu data transfer - { - if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) - { - // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). - // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. - ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); - printPicoScanImuDisabledWarning(); - imu_enable = false; - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); - } - } - if (!sendRun()) - { - return false; - } - if (!sendAuthorization()) - { - return false; - } - if (!sendSopasCmdCheckResponse("sMN LMCstartmeas", "sAN LMCstartmeas")) // start measurement - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sMN LMCstartmeas\") failed."); - return false; - } - return true; -} -#endif // SCANSEGMENT_XD_SUPPORT - -#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 -/*! - * Sends the multiScan136 stop commands "sWN ScanDataEnable 0" and "sMN Run" - */ -bool sick_scan_xd::SickScanServices::sendMultiScanStopCmd(bool imu_enable) -{ - if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 0", "sWA ScanDataEnable")) // disable scan data output - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 0\") failed."); - return false; - } - if (imu_enable && !sendSopasCmdCheckResponse("sWN ImuDataEnable 0", "sWA ImuDataEnable")) // disable imu data output - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 0\") failed."); - return false; - } - if (!sendRun()) - { - return false; - } - return true; -} -#endif // SCANSEGMENT_XD_SUPPORT - -union FLOAT_BYTE32_UNION -{ - uint8_t u8_bytes[4]; - uint32_t u32_bytes; - int32_t i32_bytes; - float value; -}; - -/*! -* Converts a hex string (hex_str: 4 byte hex value as string, little or big endian) to float. -* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html -* Examples: -* convertHexStringToFloat("C0490FF9", true) returns -3.14 -* convertHexStringToFloat("3FC90FF9", true) returns +1.57 -*/ -float sick_scan_xd::SickScanServices::convertHexStringToFloat(const std::string& hex_str, bool hexStrIsBigEndian) -{ - FLOAT_BYTE32_UNION hex_buffer; - if(hexStrIsBigEndian) - { - for(int m = 3, n = 1; n < hex_str.size(); n+=2, m--) - { - char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' }; - hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); - } - } - else - { - for(int m = 0, n = 1; n < hex_str.size(); n+=2, m++) - { - char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' }; - hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); - } - } - // ROS_DEBUG_STREAM("convertHexStringToFloat(" << hex_str << ", " << hexStrIsBigEndian << "): " << std::hex << hex_buffer.u32_bytes << " = " << std::fixed << hex_buffer.value); - return hex_buffer.value; -} - -/*! -* Converts a float value to hex string (hex_str: 4 byte hex value as string, little or big endian). -* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html -* Examples: -* convertFloatToHexString(-3.14, true) returns "C0490FDB" -* convertFloatToHexString(+1.57, true) returns "3FC90FF8" -*/ -std::string sick_scan_xd::SickScanServices::convertFloatToHexString(float value, bool hexStrIsBigEndian) -{ - FLOAT_BYTE32_UNION hex_buffer; - hex_buffer.value = value; - std::stringstream hex_str; - if(hexStrIsBigEndian) - { - for(int n = 3; n >= 0; n--) - hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); - } - else - { - for(int n = 0; n < 4; n++) - hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); - } - // ROS_DEBUG_STREAM("convertFloatToHexString(" << value << ", " << hexStrIsBigEndian << "): " << hex_str.str()); - return hex_str.str(); -} - -/*! -* Converts a hex string coded in 1/10000 deg (hex_str: 4 byte hex value as string, little or big endian) to an angle in [deg] (float). -*/ -float sick_scan_xd::SickScanServices::convertHexStringToAngleDeg(const std::string& hex_str, bool hexStrIsBigEndian) -{ - char hex_str_8byte[9] = "00000000"; - for(int m=7,n=hex_str.size()-1; n >= 0; m--,n--) - hex_str_8byte[m] = hex_str[n]; // fill with leading '0' - FLOAT_BYTE32_UNION hex_buffer; - if(hexStrIsBigEndian) - { - for(int m = 3, n = 1; n < 8; n+=2, m--) - { - char hexval[4] = { hex_str_8byte[n-1], hex_str_8byte[n], '\0', '\0' }; - hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); - } - } - else - { - for(int m = 0, n = 1; n < 8; n+=2, m++) - { - char hexval[4] = { hex_str_8byte[n-1], hex_str_8byte[n], '\0', '\0' }; - hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); - } - } - float angle_deg = (float)(hex_buffer.i32_bytes / 10000.0); - // ROS_DEBUG_STREAM("convertHexStringToAngleDeg(" << hex_str << ", " << hexStrIsBigEndian << "): " << angle_deg << " [deg]"); - return angle_deg; -} - -/*! -* Converts an angle in [deg] to hex string coded in 1/10000 deg (hex_str: 4 byte hex value as string, little or big endian). -*/ -std::string sick_scan_xd::SickScanServices::convertAngleDegToHexString(float angle_deg, bool hexStrIsBigEndian) -{ - int32_t angle_val = (int32_t)std::round(angle_deg * 10000.0f); - FLOAT_BYTE32_UNION hex_buffer; - hex_buffer.i32_bytes = angle_val; - std::stringstream hex_str; - if(hexStrIsBigEndian) - { - for(int n = 3; n >= 0; n--) - hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); - } - else - { - for(int n = 0; n < 4; n++) - hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); - } - // ROS_DEBUG_STREAM("convertAngleDegToHexString(" << angle_deg << " [deg], " << hexStrIsBigEndian << "): " << hex_str.str()); - return hex_str.str(); -} - -#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 -/*! -* Sends the SOPAS command to query multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) -* @param[out] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) -* @param[out] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree -* @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled -* @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter -*/ -bool sick_scan_xd::SickScanServices::queryMultiScanFiltersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, - sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings, const std::string& scanner_type) -{ - std::vector> sopasRepliesBin; - std::vector sopasRepliesString; - - // Query FREchoFilter, LFPangleRangeFilter and LFPlayerFilter settings - bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true; - if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter and LFPlayerFilter not supported by picoscan150 - { - // enableLFPangleRangeFilter = false; - enableLFPlayerFilter = false; - } - std::vector sopasCommands; - if (enableFREchoFilter) - sopasCommands.push_back("FREchoFilter"); - if (enableLFPangleRangeFilter) - sopasCommands.push_back("LFPangleRangeFilter"); - if (enableLFPlayerFilter) - sopasCommands.push_back("LFPlayerFilter"); - for(int n = 0; n < sopasCommands.size(); n++) - { - std::string sopasRequest = "sRN " + sopasCommands[n]; - std::string sopasExpectedResponse = "sRA " + sopasCommands[n]; - std::vector sopasReplyBin; - std::string sopasReplyString; - if(!sendSopasAndCheckAnswer(sopasRequest, sopasReplyBin, sopasReplyString) || sopasReplyString.find(sopasExpectedResponse) == std::string::npos) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): sendSopasAndCheckAnswer(\"" << sopasRequest << "\") failed or unexpected response: \"" << sopasReplyString << "\", expected: \"" << sopasExpectedResponse << "\""); - return false; - } - ROS_DEBUG_STREAM("SickScanServices::queryMultiScanFiltersettings(): request: \"" << sopasRequest << "\", response: \"" << sopasReplyString << "\""); - sopasRepliesBin.push_back(sopasReplyBin); - sopasRepliesString.push_back(sopasReplyString); - } - - // Convert sopas answers - std::vector> sopasTokens; - for(int n = 0; n < sopasCommands.size(); n++) - { - std::string parameterString = sopasRepliesString[n].substr(4 + sopasCommands[n].size() + 1); - std::vector parameterToken; - sick_scansegment_xd::util::parseVector(parameterString, parameterToken, ' '); - sopasTokens.push_back(parameterToken); - ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): " << sopasCommands[n] << ": \"" << parameterString << "\" = {" << sick_scansegment_xd::util::printVector(parameterToken, ",") << "}"); - } - - std::vector multiscan_angles_deg; - std::vector layer_active_vector; - for(int sopasCommandCnt = 0; sopasCommandCnt < sopasCommands.size(); sopasCommandCnt++) - { - const std::string& sopasCommand = sopasCommands[sopasCommandCnt]; - const std::vector& sopasToken = sopasTokens[sopasCommandCnt]; - - if (sopasCommand == "FREchoFilter") // Parse FREchoFilter - { - if(sopasToken.size() == 1) - { - host_FREchoFilter = std::stoi(sopasToken[0]); - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in FREchoFilter"); - return false; - } - } - - if (sopasCommand == "LFPangleRangeFilter") // Parse LFPangleRangeFilter - { - if(sopasToken.size() == 6) - { - std::stringstream parameter; - int filter_enabled = std::stoi(sopasToken[0]); // - parameter << filter_enabled; - for(int n = 1; n < 5; n++) // - { - // float angle_deg = (convertHexStringToFloat(sopasToken[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN) * 180.0 / M_PI); - float angle_deg = convertHexStringToAngleDeg(sopasToken[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); - parameter << " " << angle_deg; - if(filter_enabled) - multiscan_angles_deg.push_back(angle_deg); - } - parameter << " " << sopasToken[5]; // - host_LFPangleRangeFilter = parameter.str(); - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPangleRangeFilter"); - return false; - } - } - - if (sopasCommand == "LFPlayerFilter") // Parse LFPlayerFilter - { - if(sopasToken.size() == 17) - { - std::stringstream parameter; - int filter_enabled = std::stoi(sopasToken[0]); // - parameter << filter_enabled; - for(int n = 1; n < sopasToken.size(); n++) - { - int layer_active = std::stoi(sopasToken[n]); - if(filter_enabled) - layer_active_vector.push_back(layer_active); - parameter << " " << layer_active; - } - host_LFPlayerFilter = parameter.str(); - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPlayerFilter"); - return false; - } - } - } - - // Set filter settings for validation of msgpack data, i.e. set config.msgpack_validator_required_echos, config.msgpack_validator_azimuth_start, config.msgpack_validator_azimuth_end, - // config.msgpack_validator_elevation_start, config.msgpack_validator_elevation_end, config.msgpack_validator_layer_filter according to the queried filter settings - if(host_FREchoFilter == 0 || host_FREchoFilter == 2) // 0: FIRST_ECHO (EchoCount=1), 2: LAST_ECHO (EchoCount=1) - { - msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0 }; // one echo with index 0 - } - else if(host_FREchoFilter == 1) // 1: ALL_ECHOS (EchoCount=3) - { - msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0, 1, 2 }; // three echos with index 0, 1, 2 - } - else - { - ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): unexpected value of FREchoFilter = " << host_FREchoFilter - << ", expected 0: FIRST_ECHO (EchoCount=1), 1: ALL_ECHOS (EchoCount=3) or 2: LAST_ECHO (EchoCount=1)"); - return false; - } - if(multiscan_angles_deg.size() == 4) // otherwise LFPangleRangeFilter disabled (-> use configured default values) - { - msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (multiscan_angles_deg[0] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (multiscan_angles_deg[1] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_elevation_start = (multiscan_angles_deg[2] * M_PI / 180); - msgpack_validator_filter_settings.msgpack_validator_elevation_end = (multiscan_angles_deg[3] * M_PI / 180); - } - if(layer_active_vector.size() == 16) // otherwise LFPlayerFilter disabled (-> use configured default values) - { - msgpack_validator_filter_settings.msgpack_validator_layer_filter = layer_active_vector; - } - - // Example: sopas.FREchoFilter = "1", sopas.LFPangleRangeFilter = "0 -180 180 -90.0002 90.0002 1", sopas.LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" - // msgpack_validator_required_echos = { 0 }, msgpack_validator_angles = { -3.14159 3.14159 -1.5708 1.5708 } [rad], msgpack_validator_layer_filter = { 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 } - ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): sopas.FREchoFilter = \"" << host_FREchoFilter - << "\", sopas.LFPangleRangeFilter = \"" << host_LFPangleRangeFilter - << "\", sopas.LFPlayerFilter = \"" << host_LFPlayerFilter << "\""); - ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): msgpack_validator_required_echos = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos) - << " }, msgpack_validator_angles = { " << msgpack_validator_filter_settings.msgpack_validator_azimuth_start << " " << msgpack_validator_filter_settings.msgpack_validator_azimuth_end - << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_start << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_end - << " } [rad], msgpack_validator_layer_filter = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_layer_filter) << " }"); - return true; -} -#endif // SCANSEGMENT_XD_SUPPORT - -#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 -/*! -* Sends the SOPAS command to write multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) -* @param[in] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) -* @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree -* @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled -* @param[in] host_LFPintervalFilter Optionally set LFPintervalFilter to " " with 1 for enabled and 0 for disabled and N to reduce output to every N-th scan -*/ -bool sick_scan_xd::SickScanServices::writeMultiScanFiltersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter, const std::string& host_LFPintervalFilter, const std::string& scanner_type) - -{ - bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true, enableLFPintervalFilter = true; - if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter, LFPlayerFilter ant LFPintervalFilter not supported by picoscan150 - { - // enableLFPangleRangeFilter = false; - enableLFPlayerFilter = false; - // enableLFPintervalFilter = false; - } - - // Write FREchoFilter - if(enableFREchoFilter && host_FREchoFilter >= 0) // otherwise not configured or supported - { - std::string sopasRequest = "sWN FREchoFilter " + std::to_string(host_FREchoFilter), sopasExpectedResponse = "sWA FREchoFilter"; - if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); - return false; - } - } - - // Write LFPangleRangeFilter - if(enableLFPangleRangeFilter && !host_LFPangleRangeFilter.empty()) // otherwise not configured or supported - { - // convert deg to rad and float to hex - std::vector parameter_token; - sick_scansegment_xd::util::parseVector(host_LFPangleRangeFilter, parameter_token, ' '); - if(parameter_token.size() != 6) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): can't split host_LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", expected 6 values separated by space"); - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings() failed."); - return false; - } - int filter_enabled = std::stoi(parameter_token[0]); // - std::vector angle_deg; - for(int n = 1; n < 5; n++) - angle_deg.push_back(std::stof(parameter_token[n])); - int beam_increment = std::stoi(parameter_token[5]); // - std::stringstream sopas_parameter; - sopas_parameter << filter_enabled; - for(int n = 0; n < angle_deg.size(); n++) - { - // sopas_parameter << " " << convertFloatToHexString(angle_deg[n] * M_PI / 180, SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); - sopas_parameter << " " << convertAngleDegToHexString(angle_deg[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); - } - sopas_parameter << " " << beam_increment; - // Write LFPangleRangeFilter - std::string sopasRequest = "sWN LFPangleRangeFilter " + sopas_parameter.str(), sopasExpectedResponse = "sWA LFPangleRangeFilter"; - if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); - return false; - } - } - - // Write LFPlayerFilter - if(enableLFPlayerFilter && !host_LFPlayerFilter.empty()) // otherwise not configured or supported - { - std::string sopasRequest = "sWN LFPlayerFilter " + host_LFPlayerFilter, sopasExpectedResponse = "sWA LFPlayerFilter"; - if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); - return false; - } - } - - // Write LFPintervalFilter - if(enableLFPintervalFilter && !host_LFPintervalFilter.empty()) // otherwise not configured or supported - { - std::string sopasRequest = "sWN LFPintervalFilter " + host_LFPintervalFilter, sopasExpectedResponse = "sWA LFPintervalFilter"; - if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); - return false; - } - } - - // Apply the settings - if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); - return false; - } - return true; -} -#endif // SCANSEGMENT_XD_SUPPORT - -/*! -* Callbacks for service messages. -* @param[in] service_request ros service request to lidar -* @param[out] service_response service response from lidar -* @return true on success, false in case of errors. -*/ -bool sick_scan_xd::SickScanServices::serviceCbSCdevicestate(sick_scan_srv::SCdevicestateSrv::Request &service_request, sick_scan_srv::SCdevicestateSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sRN SCdevicestate"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - service_response.state = 2; // ERROR - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - std::string response_str((char*)sopasReplyBin.data(), sopasReplyBin.size()); - std::size_t state_pos = response_str.find("SCdevicestate"); - if (state_pos != std::string::npos && state_pos + 14 < sopasReplyBin.size()) - { - uint8_t state_byte = sopasReplyBin[state_pos + 14]; - if(state_byte >= '0') - state_byte -= '0'; - service_response.state = state_byte; - } - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\" = \"" << DataDumper::binDataToAsciiString(sopasReplyBin.data(), sopasReplyBin.size()) << "\""); - - return true; -} - -bool sick_scan_xd::SickScanServices::serviceCbSCreboot(sick_scan_srv::SCrebootSrv::Request &service_request, sick_scan_srv::SCrebootSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sMN mSCreboot"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - if(!sendAuthorization()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); - return false; - } - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - if(!sendRun()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - return true; -} - -bool sick_scan_xd::SickScanServices::serviceCbSCsoftreset(sick_scan_srv::SCsoftresetSrv::Request &service_request, sick_scan_srv::SCsoftresetSrv::Response &service_response) -{ - std::string sopasCmd = std::string("sMN mSCsoftreset"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - if(!sendAuthorization()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); - return false; - } - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - if(!sendRun()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); - return false; - } - service_response.success = true; - - return true; -} - -bool sick_scan_xd::SickScanServices::serviceCbSickScanExit(sick_scan_srv::SickScanExitSrv::Request &service_request, sick_scan_srv::SickScanExitSrv::Response &service_response) -{ - /* - std::string sopasCmd = std::string("sMN mSCsoftreset"); - std::vector sopasReplyBin; - std::string sopasReplyString; - - service_response.success = false; - if(!sendAuthorization()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); - return false; - } - - if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) - { - ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); - return false; - } - - ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); - ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); - - if(!sendRun()) - { - ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); - return false; - } - */ - service_response.success = stopScannerAndExit(false); - return true; -} - -#if __ROS_VERSION > 0 -bool sick_scan_xd::SickScanServices::serviceCbFieldSetRead(sick_scan_srv::FieldSetReadSrv::Request &service_request, sick_scan_srv::FieldSetReadSrv::Response &service_response) -{ - SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); - int field_set_selection_method = fieldMon->getFieldSelectionMethod(); - int active_field_set = fieldMon->getActiveFieldset(); - std::vector sopasReply; - int result1 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); - int result2 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); - service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess); - service_response.field_set_selection_method = field_set_selection_method; - service_response.active_field_set = active_field_set; - return true; -} - -bool sick_scan_xd::SickScanServices::serviceCbFieldSetWrite(sick_scan_srv::FieldSetWriteSrv::Request &service_request, sick_scan_srv::FieldSetWriteSrv::Response &service_response) -{ - int field_set_selection_method = service_request.field_set_selection_method_in; - int active_field_set = service_request.active_field_set_in; - std::vector sopasReply; - int result1 = ExitSuccess, result2 = ExitSuccess; - if (field_set_selection_method >= 0) - { - result1 = m_common_tcp->writeFieldSetSelectionMethod(field_set_selection_method, sopasReply); - } - if (active_field_set >= 0) - { - result2 = m_common_tcp->writeActiveFieldSet(active_field_set, sopasReply); - } - int result3 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); - int result4 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); - service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess && result3 == ExitSuccess && result4 == ExitSuccess); - service_response.field_set_selection_method = field_set_selection_method; - service_response.active_field_set = active_field_set; - service_response.success = true; - return true; -} -#endif +/* + * @brief Implementation of ROS services for sick_scan + * + * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2021, SICK AG, Waldkirch + * All rights reserved. + * +* 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. +* +* +* 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 Osnabrueck University nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* * Neither the name of SICK AG nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission +* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. + * + * Created on: 12.01.2021 + * + * Authors: + * Michael Lehning + * + * Based on the TiM communication example by SICK AG. + * + */ +#include +#include + +#include "sick_scan/sick_scan_services.h" +#include "sick_scan/sick_generic_laser.h" +#include "sick_scansegment_xd/udp_poster.h" + +#define SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN (true) // Arguments of SOPAS commands are big endian encoded + +sick_scan_xd::SickScanServices::SickScanServices(rosNodePtr nh, sick_scan_xd::SickScanCommonTcp* common_tcp, ScannerBasicParam * lidar_param) +: m_common_tcp(common_tcp), m_cola_binary(true) +{ + bool srvSupportColaMsg = true, srvSupportECRChangeArr = true, srvSupportLIDoutputstate = true, srvSupportSCdevicestate = true; + bool srvSupportSCreboot = true, srvSupportSCsoftreset = true, srvSupportSickScanExit = true; + bool srvSupportGetContaminationResult = false; + if(lidar_param) + { + m_cola_binary = lidar_param->getUseBinaryProtocol(); + if(lidar_param->getScannerName().compare(SICK_SCANNER_NAV_350_NAME) == 0) + { + srvSupportECRChangeArr = false; + srvSupportLIDoutputstate = false; + srvSupportSCreboot = false; + srvSupportSCsoftreset = false; + } + if(lidar_param->getScannerName().compare(SICK_SCANNER_MRS_1XXX_NAME) == 0 + || lidar_param->getScannerName().compare(SICK_SCANNER_LMS_1XXX_NAME) == 0 + || lidar_param->getScannerName().compare(SICK_SCANNER_SCANSEGMENT_XD_NAME) == 0) + { + srvSupportGetContaminationResult = true; // "sRN ContaminationResult" supported by MRS-1000, LMS-1000, multiScan + } + } + if(nh) + { + m_client_authorization_pw = "F4724744"; + if (lidar_param->getUseSafetyPasWD()) // TIM_7xxS - 1 layer Safety Scanner + m_client_authorization_pw = "6FD62C05"; + rosDeclareParam(nh, "client_authorization_pw", m_client_authorization_pw); + rosGetParam(nh, "client_authorization_pw", m_client_authorization_pw); + +#if __ROS_VERSION == 2 +#define serviceCbColaMsgROS sick_scan_xd::SickScanServices::serviceCbColaMsgROS2 +#define serviceCbECRChangeArrROS sick_scan_xd::SickScanServices::serviceCbECRChangeArrROS2 +#define serviceCbLIDoutputstateROS sick_scan_xd::SickScanServices::serviceCbLIDoutputstateROS2 +#define serviceCbSCdevicestateROS sick_scan_xd::SickScanServices::serviceCbSCdevicestateROS2 +#define serviceCbSCrebootROS sick_scan_xd::SickScanServices::serviceCbSCrebootROS2 +#define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftresetROS2 +#define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExitROS2 +#define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResultROS2 +#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetReadROS2 +#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWriteROS2 +#else +#define serviceCbColaMsgROS sick_scan_xd::SickScanServices::serviceCbColaMsg +#define serviceCbECRChangeArrROS sick_scan_xd::SickScanServices::serviceCbECRChangeArr +#define serviceCbLIDoutputstateROS sick_scan_xd::SickScanServices::serviceCbLIDoutputstate +#define serviceCbSCdevicestateROS sick_scan_xd::SickScanServices::serviceCbSCdevicestate +#define serviceCbSCrebootROS sick_scan_xd::SickScanServices::serviceCbSCreboot +#define serviceCbSCsoftresetROS sick_scan_xd::SickScanServices::serviceCbSCsoftreset +#define serviceCbSickScanExitROS sick_scan_xd::SickScanServices::serviceCbSickScanExit +#define serviceCbGetContaminationResultROS sick_scan_xd::SickScanServices::serviceCbGetContaminationResult +#define serviceCbFieldSetReadROS sick_scan_xd::SickScanServices::serviceCbFieldSetRead +#define serviceCbFieldSetWriteROS sick_scan_xd::SickScanServices::serviceCbFieldSetWrite +#endif +#if __ROS_VERSION == 1 +#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a.getService() << "\" created (\"" << b.getService() << "\")"); +#elif __ROS_VERSION == 2 +#define printServiceCreated(a,b) ROS_INFO_STREAM("SickScanServices: service \"" << a->get_service_name() << "\" created (\"" << b->get_service_name() << "\")"); +#else +#define printServiceCreated(a,b) +#endif + if(srvSupportColaMsg) + { + auto srv_server_ColaMsg = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ColaMsgSrv, "ColaMsg", &serviceCbColaMsgROS, this); + m_srv_server_ColaMsg = rosServiceServer(srv_server_ColaMsg); + printServiceCreated(srv_server_ColaMsg, m_srv_server_ColaMsg); + } + if(srvSupportECRChangeArr) + { + auto srv_server_ECRChangeArr = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::ECRChangeArrSrv, "ECRChangeArr", &serviceCbECRChangeArrROS, this); + m_srv_server_ECRChangeArr = rosServiceServer(srv_server_ECRChangeArr); + printServiceCreated(srv_server_ECRChangeArr, m_srv_server_ECRChangeArr); + } + if(srvSupportGetContaminationResult) + { + auto srv_server_GetContaminationResult = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::GetContaminationResultSrv, "GetContaminationResult", &serviceCbGetContaminationResultROS, this); + m_srv_server_GetContaminationResult = rosServiceServer(srv_server_GetContaminationResult); + printServiceCreated(srv_server_GetContaminationResult, m_srv_server_GetContaminationResult); + } + if(srvSupportLIDoutputstate) + { + auto srv_server_LIDoutputstate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::LIDoutputstateSrv, "LIDoutputstate", &serviceCbLIDoutputstateROS, this); + m_srv_server_LIDoutputstate = rosServiceServer(srv_server_LIDoutputstate); + printServiceCreated(srv_server_LIDoutputstate, m_srv_server_LIDoutputstate); + } + if(srvSupportSCdevicestate) + { + auto srv_server_SCdevicestate = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCdevicestateSrv, "SCdevicestate", &serviceCbSCdevicestateROS, this); + m_srv_server_SCdevicestate = rosServiceServer(srv_server_SCdevicestate); + printServiceCreated(srv_server_SCdevicestate, m_srv_server_SCdevicestate); + } + if(srvSupportSCreboot) + { + auto srv_server_SCreboot = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCrebootSrv, "SCreboot", &serviceCbSCrebootROS, this); + m_srv_server_SCreboot = rosServiceServer(srv_server_SCreboot); + printServiceCreated(srv_server_SCreboot, m_srv_server_SCreboot); + } + if(srvSupportSCsoftreset) + { + auto srv_server_SCsoftreset = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SCsoftresetSrv, "SCsoftreset", &serviceCbSCsoftresetROS, this); + m_srv_server_SCsoftreset = rosServiceServer(srv_server_SCsoftreset); + printServiceCreated(srv_server_SCsoftreset, m_srv_server_SCsoftreset); + } + if(srvSupportSickScanExit) + { + auto srv_server_SickScanExit = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::SickScanExitSrv, "SickScanExit", &serviceCbSickScanExitROS, this); + m_srv_server_SickScanExit = rosServiceServer(srv_server_SickScanExit); + printServiceCreated(srv_server_SickScanExit, m_srv_server_SickScanExit); + } +#if __ROS_VERSION > 0 + if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + auto srv_server_FieldSetRead = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetReadSrv, "FieldSetRead", &serviceCbFieldSetReadROS, this); + m_srv_server_FieldSetRead = rosServiceServer(srv_server_FieldSetRead); + printServiceCreated(srv_server_FieldSetRead, m_srv_server_FieldSetRead); + } + if(lidar_param->getUseEvalFields() == USE_EVAL_FIELD_TIM7XX_LOGIC) + { + auto srv_server_FieldSetWrite = ROS_CREATE_SRV_SERVER(nh, sick_scan_srv::FieldSetWriteSrv, "FieldSetWrite", &serviceCbFieldSetWriteROS, this); + m_srv_server_FieldSetWrite = rosServiceServer(srv_server_FieldSetWrite); + printServiceCreated(srv_server_FieldSetWrite, m_srv_server_FieldSetWrite); + } +#endif + } +} + +sick_scan_xd::SickScanServices::~SickScanServices() +{ +} + +/*! + * Sends a sopas command and returns the lidar reply. + * @param[in] sopasCmd sopas command to send, f.e. "sEN ECRChangeArr 1" + * @param[out] sopasReplyBin response from lidar incl. start/stop byte + * @param[out] sopasReplyString sopasReplyBin converted to string + * @return true on success, false in case of errors. + */ +bool sick_scan_xd::SickScanServices::sendSopasAndCheckAnswer(const std::string& sopasCmd, std::vector& sopasReplyBin, std::string& sopasReplyString) +{ + if(m_common_tcp) + { + ROS_INFO_STREAM("SickScanServices: Sending request \"" << sopasCmd << "\""); + std::string sopasRequest = std::string("\x02") + sopasCmd + "\x03"; + int result = -1; + if (m_cola_binary) + { + std::vector reqBinary; + m_common_tcp->convertAscii2BinaryCmd(sopasRequest.c_str(), &reqBinary); + result = m_common_tcp->sendSopasAndCheckAnswer(reqBinary, &sopasReplyBin, -1); + } + else + { + result = m_common_tcp->sendSopasAndCheckAnswer(sopasRequest.c_str(), &sopasReplyBin, -1); + } + if (result != 0) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: error sending sopas command \"" << sopasCmd << "\""); + } + else + { + sopasReplyString = m_common_tcp->sopasReplyToString(sopasReplyBin); + ROS_INFO_STREAM("SickScanServices: Request \"" << sopasCmd << "\" successfully sent, received reply \"" << sopasReplyString << "\""); + return true; + } + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer: m_common_tcp not initialized"); + } + return false; +} + +/*! + * Callback for service ColaMsg (ColaMsg, send a cola message to lidar). + * @param[in] service_request ros service request to lidar + * @param[out] service_response service response from lidar + * @return true on success, false in case of errors. + */ +bool sick_scan_xd::SickScanServices::serviceCbColaMsg(sick_scan_srv::ColaMsgSrv::Request &service_request, sick_scan_srv::ColaMsgSrv::Response &service_response) +{ + std::string sopasCmd = service_request.request; + std::vector sopasReplyBin; + std::string sopasReplyString; + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + service_response.response = sopasReplyString; + return true; +} + +/*! + * Callback for service messages (ECRChangeArr, Request status change of monitoring fields on event). + * Sends a cola telegram "sEN ECRChangeArr {0|1}" and receives the response from the lidar device. + * @param[in] service_request ros service request to lidar + * @param[out] service_response service response from lidar + * @return true on success, false in case of errors. + */ +bool sick_scan_xd::SickScanServices::serviceCbECRChangeArr(sick_scan_srv::ECRChangeArrSrv::Request &service_request, sick_scan_srv::ECRChangeArrSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sEN ECRChangeArr ") + (service_request.active ? "1" : "0"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + return true; +} + +/*! +* Callbacks for service messages. +* @param[in] service_request ros service request to lidar +* @param[out] service_response service response from lidar +* @return true on success, false in case of errors. +*/ +bool sick_scan_xd::SickScanServices::serviceCbGetContaminationResult(sick_scan_srv::GetContaminationResultSrv::Request &service_request, sick_scan_srv::GetContaminationResultSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sRN ContaminationResult"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + service_response.warning = 0; + service_response.error = 0; + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + std::string response_str((char*)sopasReplyBin.data(), sopasReplyBin.size()); + std::size_t state_pos = response_str.find("ContaminationResult"); + int result_idx = 20; + if (state_pos != std::string::npos && state_pos + result_idx < sopasReplyBin.size()) + { + uint8_t result_byte = sopasReplyBin[state_pos + result_idx]; + result_byte = ((result_byte >= '0') ? (result_byte - '0') : (result_byte)); // convert to bin in case of ascii + service_response.warning = result_byte; + result_idx++; + if (result_idx < sopasReplyBin.size() && sopasReplyBin[state_pos + result_idx] == ' ') // jump over ascii separator + result_idx++; + if (result_idx < sopasReplyBin.size()) + { + result_byte = sopasReplyBin[state_pos + result_idx]; + result_byte = ((result_byte >= '0') ? (result_byte - '0') : (result_byte)); // convert to bin in case of ascii + service_response.error = result_byte; + } + } + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\" = \"" << DataDumper::binDataToAsciiString(sopasReplyBin.data(), sopasReplyBin.size()) << "\"" + << " (response.success=" << (int)(service_response.success) << ", response.warning=" << (int)(service_response.warning) << ", response.error=" << (int)(service_response.error) << ")"); + + return true; +} + + +/*! + * Callback for service messages (LIDoutputstate, Request status change of monitoring fields on event). + * Sends a cola telegram "sEN LIDoutputstate {0|1}" and receives the response from the lidar device. + * @param[in] service_request ros service request to lidar + * @param[out] service_response service response from lidar + * @return true on success, false in case of errors. + */ +bool sick_scan_xd::SickScanServices::serviceCbLIDoutputstate(sick_scan_srv::LIDoutputstateSrv::Request &service_request, sick_scan_srv::LIDoutputstateSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sEN LIDoutputstate ") + (service_request.active ? "1" : "0"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + return true; +} + +/*! + * Sends the SOPAS authorization command "sMN SetAccessMode 3 F4724744". + */ +bool sick_scan_xd::SickScanServices::sendAuthorization() +{ + std::string sopasCmd = std::string("sMN SetAccessMode 3 ") + m_client_authorization_pw; + std::vector sopasReplyBin; + std::string sopasReplyString; + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + return true; +} + +/*! + * Sends the SOPAS command "sMN Run", which applies previous send settings + */ +bool sick_scan_xd::SickScanServices::sendRun() +{ + std::string sopasCmd = std::string("sMN Run"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + return true; +} + +/*! + * Sends a multiScan136 command + */ +bool sick_scan_xd::SickScanServices::sendSopasCmdCheckResponse(const std::string& sopas_request, const std::string& expected_response) +{ + std::vector sopasReplyBin; + std::string sopasReplyString; + if(!sendSopasAndCheckAnswer(sopas_request, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse() failed on sending command\"" << sopas_request << "\""); + return false; + } + ROS_INFO_STREAM("SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", response: \"" << sopasReplyString << "\""); + if(sopasReplyString.find(expected_response) == std::string::npos) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasCmdCheckResponse(): request: \"" << sopas_request << "\", unexpected response: \"" << sopasReplyString << "\", \"" << expected_response << "\" not found"); + return false; + } + return true; +} + +#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 +static void printPicoScanImuDisabledWarning() +{ + ROS_WARN_STREAM("############################"); + ROS_WARN_STREAM("## ##"); + ROS_WARN_STREAM("## IMU will be disabled ##"); + ROS_WARN_STREAM("## ##"); + ROS_WARN_STREAM("############################"); + ROS_WARN_STREAM("## If you are using a picoScan150 w/o addons, disable the IMU by setting option \"imu_enable\" to \"False\" in your launchfile, or use sick_picoscan_single_echo.launch to avoid this error"); + ROS_WARN_STREAM("## If you are using a picoScan with IMU, check IMU settings with SOPAS Air"); +} + +/*! +* Sends the multiScan start commands "sWN ScanDataFormat", "sWN ScanDataPreformatting", "sWN ScanDataEthSettings", "sWN ScanDataEnable 1", "sMN LMCstartmeas", "sMN Run" +* @param[in] hostname IP address of multiScan136, default 192.168.0.1 +* @param[in] port IP port of multiScan136, default 2115 +* @param[in] scanner_type type of scanner, currently supported are multiScan136 and picoScan150 +* @param[in] scandataformat ScanDataFormat: 1 for msgpack or 2 for compact scandata, default: 2 +* @param[in+out] imu_enable: Imu data transfer enabled +* @param[in] imu_udp_port: UDP port of imu data (if imu_enable is true) +* @param[in] check_udp_receiver_ip: check udp_receiver_ip by sending and receiving a udp test message +* @param[in] check_udp_receiver_port: udp port to check udp_receiver_ip +*/ +bool sick_scan_xd::SickScanServices::sendMultiScanStartCmd(const std::string& hostname, int port, const std::string& scanner_type, int scandataformat, bool& imu_enable, int imu_udp_port, int performanceprofilenumber, bool check_udp_receiver_ip, int check_udp_receiver_port) +{ + // Check udp receiver ip address: hostname is expected to be a IPv4 address + std::stringstream ip_stream(hostname); + std::string ip_token; + std::vector ip_tokens; + while (getline(ip_stream, ip_token, '.')) + { + ip_tokens.push_back(ip_token); + } + if (ip_tokens.size() != 4) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed: can't split ip address \"" << hostname << "\" into 4 tokens, check ip address."); + ROS_ERROR_STREAM("## ERROR parsing ip address, check configuration of parameter udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); + ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); + return false; + } + // Check udp receiver ip address by sending and receiving a UDP test message + std::string check_udp_receiver_msg = "sick_scan_xd udp test message verifying udp_receiver_ip"; + if (check_udp_receiver_ip && !check_udp_receiver_msg.empty()) + { + sick_scansegment_xd::UdpPoster udp_loopback_poster(hostname, check_udp_receiver_port); + std::string check_udp_receiver_response; + if (!udp_loopback_poster.Post(check_udp_receiver_msg, check_udp_receiver_response) || check_udp_receiver_msg != check_udp_receiver_response) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd() failed to send and receive a UDP test messsage to " << hostname << ":" << check_udp_receiver_port); + ROS_ERROR_STREAM("## Check configuration of parameter udp_receiver_ip=\"" << hostname << "\" (launch file or commandline option)."); + ROS_ERROR_STREAM("## Parameter \"udp_receiver_ip\" should be the IPv4 address like 192.168.0.x of the system running sick_scan_xd."); + return false; + } + else + { + ROS_DEBUG_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message \"" << check_udp_receiver_response << "\" successfully received."); + ROS_INFO_STREAM("SickScanServices::sendMultiScanStartCmd(): UDP test message successfully send to " << hostname << ":" << check_udp_receiver_port << ", parameter udp_receiver_ip=\"" << hostname << "\" is valid."); + } + } + // Send sopas start sequence (ScanDataFormat, ScanDataEthSettings, ImuDataEthSettings, ScanDataEnable, ImuDataEnable) + std::stringstream eth_settings_cmd, imu_eth_settings_cmd, scandataformat_cmd, performanceprofilenumber_cmd; + scandataformat_cmd << "sWN ScanDataFormat " << scandataformat; + if (performanceprofilenumber >= 0) + { + performanceprofilenumber_cmd << "sWN PerformanceProfileNumber " << std::uppercase << std::hex << performanceprofilenumber; + } + eth_settings_cmd << "sWN ScanDataEthSettings 1"; + imu_eth_settings_cmd << "sWN ImuDataEthSettings 1"; + for (int i = 0; i < ip_tokens.size(); i++) + { + eth_settings_cmd << " +"; + eth_settings_cmd << ip_tokens[i]; + imu_eth_settings_cmd << " +"; + imu_eth_settings_cmd << ip_tokens[i]; + } + eth_settings_cmd << " +"; + eth_settings_cmd << port; + imu_eth_settings_cmd << " +"; + imu_eth_settings_cmd << imu_udp_port; + if (!sendSopasCmdCheckResponse(eth_settings_cmd.str(), "sWA ScanDataEthSettings")) // configure destination scan data output destination , f.e. "sWN ScanDataEthSettings 1 +192 +168 +0 +52 +2115" (ip 192.168.0.52 port 2115) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEthSettings 1\") failed."); + return false; + } + if (scandataformat != 1 && scandataformat != 2) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): invalid scandataformat configuration, unsupported scandataformat=" << scandataformat << ", check configuration and use 1 for msgpack or 2 for compact data"); + return false; + } + if (!sendSopasCmdCheckResponse(scandataformat_cmd.str(), "sWA ScanDataFormat")) // set scan data output format to MSGPACK (1) or COMPACT (2) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataFormat 1\") failed."); + return false; + } + if (performanceprofilenumber >= 0) + { + if (!sendSopasCmdCheckResponse(performanceprofilenumber_cmd.str(), "sWA PerformanceProfileNumber")) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN PerformanceProfileNumber ..\") failed."); + return false; + } + } + if (scanner_type == SICK_SCANNER_SCANSEGMENT_XD_NAME && !sendSopasCmdCheckResponse("sWN ScanDataPreformatting 1", "sWA ScanDataPreformatting")) // ScanDataPreformatting for multiScan136 only + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiscanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataPreformatting 1\") failed."); + return false; + } + if (imu_enable && !sendSopasCmdCheckResponse(imu_eth_settings_cmd.str(), "sWA ImuDataEthSettings")) // imu data eth settings + { + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) + { + // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). + // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. + ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); + printPicoScanImuDisabledWarning(); + imu_enable = false; + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"" << imu_eth_settings_cmd.str() << "\") failed."); + } + } + if (!sendRun()) + { + return false; + } + if (!sendAuthorization()) + { + return false; + } + if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 1", "sWA ScanDataEnable")) // enable scan data output + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 1\") failed."); + return false; + } + if (imu_enable && !sendSopasCmdCheckResponse("sWN ImuDataEnable 1", "sWA ImuDataEnable")) // enable imu data transfer + { + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) + { + // picoScan150 ships in 2 variants, with and without IMU resp. IMU licence (picoScan150 w/o addons). + // For picoScan150 w/o addons we disable the IMU, if SOPAS commands "ImuDataEthSettings" or "ImuDataEnable" failed. + ROS_WARN_STREAM("## WARNING SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); + printPicoScanImuDisabledWarning(); + imu_enable = false; + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 1\") failed."); + } + } + if (!sendRun()) + { + return false; + } + if (!sendAuthorization()) + { + return false; + } + if (!sendSopasCmdCheckResponse("sMN LMCstartmeas", "sAN LMCstartmeas")) // start measurement + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStartCmd(): sendSopasCmdCheckResponse(\"sMN LMCstartmeas\") failed."); + return false; + } + return true; +} +#endif // SCANSEGMENT_XD_SUPPORT + +#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 +/*! + * Sends the multiScan136 stop commands "sWN ScanDataEnable 0" and "sMN Run" + */ +bool sick_scan_xd::SickScanServices::sendMultiScanStopCmd(bool imu_enable) +{ + if (!sendSopasCmdCheckResponse("sWN ScanDataEnable 0", "sWA ScanDataEnable")) // disable scan data output + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sWN ScanDataEnable 0\") failed."); + return false; + } + if (imu_enable && !sendSopasCmdCheckResponse("sWN ImuDataEnable 0", "sWA ImuDataEnable")) // disable imu data output + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendMultiScanStopCmd(): sendSopasCmdCheckResponse(\"sWN ImuDataEnable 0\") failed."); + return false; + } + if (!sendRun()) + { + return false; + } + return true; +} +#endif // SCANSEGMENT_XD_SUPPORT + +union FLOAT_BYTE32_UNION +{ + uint8_t u8_bytes[4]; + uint32_t u32_bytes; + int32_t i32_bytes; + float value; +}; + +/*! +* Converts a hex string (hex_str: 4 byte hex value as string, little or big endian) to float. +* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html +* Examples: +* convertHexStringToFloat("C0490FF9", true) returns -3.14 +* convertHexStringToFloat("3FC90FF9", true) returns +1.57 +*/ +float sick_scan_xd::SickScanServices::convertHexStringToFloat(const std::string& hex_str, bool hexStrIsBigEndian) +{ + FLOAT_BYTE32_UNION hex_buffer; + if(hexStrIsBigEndian) + { + for(int m = 3, n = 1; n < hex_str.size(); n+=2, m--) + { + char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' }; + hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); + } + } + else + { + for(int m = 0, n = 1; n < hex_str.size(); n+=2, m++) + { + char hexval[4] = { hex_str[n-1], hex_str[n], '\0', '\0' }; + hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); + } + } + // ROS_DEBUG_STREAM("convertHexStringToFloat(" << hex_str << ", " << hexStrIsBigEndian << "): " << std::hex << hex_buffer.u32_bytes << " = " << std::fixed << hex_buffer.value); + return hex_buffer.value; +} + +/*! +* Converts a float value to hex string (hex_str: 4 byte hex value as string, little or big endian). +* Check f.e. by https://www.h-schmidt.net/FloatConverter/IEEE754.html +* Examples: +* convertFloatToHexString(-3.14, true) returns "C0490FDB" +* convertFloatToHexString(+1.57, true) returns "3FC90FF8" +*/ +std::string sick_scan_xd::SickScanServices::convertFloatToHexString(float value, bool hexStrIsBigEndian) +{ + FLOAT_BYTE32_UNION hex_buffer; + hex_buffer.value = value; + std::stringstream hex_str; + if(hexStrIsBigEndian) + { + for(int n = 3; n >= 0; n--) + hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); + } + else + { + for(int n = 0; n < 4; n++) + hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); + } + // ROS_DEBUG_STREAM("convertFloatToHexString(" << value << ", " << hexStrIsBigEndian << "): " << hex_str.str()); + return hex_str.str(); +} + +/*! +* Converts a hex string coded in 1/10000 deg (hex_str: 4 byte hex value as string, little or big endian) to an angle in [deg] (float). +*/ +float sick_scan_xd::SickScanServices::convertHexStringToAngleDeg(const std::string& hex_str, bool hexStrIsBigEndian) +{ + char hex_str_8byte[9] = "00000000"; + for(int m=7,n=hex_str.size()-1; n >= 0; m--,n--) + hex_str_8byte[m] = hex_str[n]; // fill with leading '0' + FLOAT_BYTE32_UNION hex_buffer; + if(hexStrIsBigEndian) + { + for(int m = 3, n = 1; n < 8; n+=2, m--) + { + char hexval[4] = { hex_str_8byte[n-1], hex_str_8byte[n], '\0', '\0' }; + hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); + } + } + else + { + for(int m = 0, n = 1; n < 8; n+=2, m++) + { + char hexval[4] = { hex_str_8byte[n-1], hex_str_8byte[n], '\0', '\0' }; + hex_buffer.u8_bytes[m] = (uint8_t)(std::strtoul(hexval, NULL, 16) & 0xFF); + } + } + float angle_deg = (float)(hex_buffer.i32_bytes / 10000.0); + // ROS_DEBUG_STREAM("convertHexStringToAngleDeg(" << hex_str << ", " << hexStrIsBigEndian << "): " << angle_deg << " [deg]"); + return angle_deg; +} + +/*! +* Converts an angle in [deg] to hex string coded in 1/10000 deg (hex_str: 4 byte hex value as string, little or big endian). +*/ +std::string sick_scan_xd::SickScanServices::convertAngleDegToHexString(float angle_deg, bool hexStrIsBigEndian) +{ + int32_t angle_val = (int32_t)std::round(angle_deg * 10000.0f); + FLOAT_BYTE32_UNION hex_buffer; + hex_buffer.i32_bytes = angle_val; + std::stringstream hex_str; + if(hexStrIsBigEndian) + { + for(int n = 3; n >= 0; n--) + hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); + } + else + { + for(int n = 0; n < 4; n++) + hex_str << std::setfill('0') << std::setw(2) << std::uppercase << std::hex << (int)(hex_buffer.u8_bytes[n]); + } + // ROS_DEBUG_STREAM("convertAngleDegToHexString(" << angle_deg << " [deg], " << hexStrIsBigEndian << "): " << hex_str.str()); + return hex_str.str(); +} + +#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 +/*! +* Sends the SOPAS command to query multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) +* @param[out] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) +* @param[out] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree +* @param[out] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled +* @param[out] msgpack_validator_filter_settings; // filter settings for msgpack validator: required_echos, azimuth_start, azimuth_end. elevation_start, elevation_end, layer_filter +*/ +bool sick_scan_xd::SickScanServices::queryMultiScanFiltersettings(int& host_FREchoFilter, std::string& host_LFPangleRangeFilter, std::string& host_LFPlayerFilter, + sick_scansegment_xd::MsgpackValidatorFilterConfig& msgpack_validator_filter_settings, const std::string& scanner_type) +{ + std::vector> sopasRepliesBin; + std::vector sopasRepliesString; + + // Query FREchoFilter, LFPangleRangeFilter and LFPlayerFilter settings + bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true; + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter and LFPlayerFilter not supported by picoscan150 + { + // enableLFPangleRangeFilter = false; + enableLFPlayerFilter = false; + } + std::vector sopasCommands; + if (enableFREchoFilter) + sopasCommands.push_back("FREchoFilter"); + if (enableLFPangleRangeFilter) + sopasCommands.push_back("LFPangleRangeFilter"); + if (enableLFPlayerFilter) + sopasCommands.push_back("LFPlayerFilter"); + for(int n = 0; n < sopasCommands.size(); n++) + { + std::string sopasRequest = "sRN " + sopasCommands[n]; + std::string sopasExpectedResponse = "sRA " + sopasCommands[n]; + std::vector sopasReplyBin; + std::string sopasReplyString; + if(!sendSopasAndCheckAnswer(sopasRequest, sopasReplyBin, sopasReplyString) || sopasReplyString.find(sopasExpectedResponse) == std::string::npos) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): sendSopasAndCheckAnswer(\"" << sopasRequest << "\") failed or unexpected response: \"" << sopasReplyString << "\", expected: \"" << sopasExpectedResponse << "\""); + return false; + } + ROS_DEBUG_STREAM("SickScanServices::queryMultiScanFiltersettings(): request: \"" << sopasRequest << "\", response: \"" << sopasReplyString << "\""); + sopasRepliesBin.push_back(sopasReplyBin); + sopasRepliesString.push_back(sopasReplyString); + } + + // Convert sopas answers + std::vector> sopasTokens; + for(int n = 0; n < sopasCommands.size(); n++) + { + std::string parameterString = sopasRepliesString[n].substr(4 + sopasCommands[n].size() + 1); + std::vector parameterToken; + sick_scansegment_xd::util::parseVector(parameterString, parameterToken, ' '); + sopasTokens.push_back(parameterToken); + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): " << sopasCommands[n] << ": \"" << parameterString << "\" = {" << sick_scansegment_xd::util::printVector(parameterToken, ",") << "}"); + } + + std::vector multiscan_angles_deg; + std::vector layer_active_vector; + for(int sopasCommandCnt = 0; sopasCommandCnt < sopasCommands.size(); sopasCommandCnt++) + { + const std::string& sopasCommand = sopasCommands[sopasCommandCnt]; + const std::vector& sopasToken = sopasTokens[sopasCommandCnt]; + + if (sopasCommand == "FREchoFilter") // Parse FREchoFilter + { + if(sopasToken.size() == 1) + { + host_FREchoFilter = std::stoi(sopasToken[0]); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in FREchoFilter"); + return false; + } + } + + if (sopasCommand == "LFPangleRangeFilter") // Parse LFPangleRangeFilter + { + if(sopasToken.size() == 6) + { + std::stringstream parameter; + int filter_enabled = std::stoi(sopasToken[0]); // + parameter << filter_enabled; + for(int n = 1; n < 5; n++) // + { + // float angle_deg = (convertHexStringToFloat(sopasToken[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN) * 180.0 / M_PI); + float angle_deg = convertHexStringToAngleDeg(sopasToken[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); + parameter << " " << angle_deg; + if(filter_enabled) + multiscan_angles_deg.push_back(angle_deg); + } + parameter << " " << sopasToken[5]; // + host_LFPangleRangeFilter = parameter.str(); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPangleRangeFilter"); + return false; + } + } + + if (sopasCommand == "LFPlayerFilter") // Parse LFPlayerFilter + { + if(sopasToken.size() == 17) + { + std::stringstream parameter; + int filter_enabled = std::stoi(sopasToken[0]); // + parameter << filter_enabled; + for(int n = 1; n < sopasToken.size(); n++) + { + int layer_active = std::stoi(sopasToken[n]); + if(filter_enabled) + layer_active_vector.push_back(layer_active); + parameter << " " << layer_active; + } + host_LFPlayerFilter = parameter.str(); + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): parse error in LFPlayerFilter"); + return false; + } + } + } + + // Set filter settings for validation of msgpack data, i.e. set config.msgpack_validator_required_echos, config.msgpack_validator_azimuth_start, config.msgpack_validator_azimuth_end, + // config.msgpack_validator_elevation_start, config.msgpack_validator_elevation_end, config.msgpack_validator_layer_filter according to the queried filter settings + if(host_FREchoFilter == 0 || host_FREchoFilter == 2) // 0: FIRST_ECHO (EchoCount=1), 2: LAST_ECHO (EchoCount=1) + { + msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0 }; // one echo with index 0 + } + else if(host_FREchoFilter == 1) // 1: ALL_ECHOS (EchoCount=3) + { + msgpack_validator_filter_settings.msgpack_validator_required_echos = { 0, 1, 2 }; // three echos with index 0, 1, 2 + } + else + { + ROS_ERROR_STREAM("## ERROR SickScanServices::queryMultiScanFiltersettings(): unexpected value of FREchoFilter = " << host_FREchoFilter + << ", expected 0: FIRST_ECHO (EchoCount=1), 1: ALL_ECHOS (EchoCount=3) or 2: LAST_ECHO (EchoCount=1)"); + return false; + } + if(multiscan_angles_deg.size() == 4) // otherwise LFPangleRangeFilter disabled (-> use configured default values) + { + msgpack_validator_filter_settings.msgpack_validator_azimuth_start = (multiscan_angles_deg[0] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_azimuth_end = (multiscan_angles_deg[1] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_elevation_start = (multiscan_angles_deg[2] * M_PI / 180); + msgpack_validator_filter_settings.msgpack_validator_elevation_end = (multiscan_angles_deg[3] * M_PI / 180); + } + if(layer_active_vector.size() == 16) // otherwise LFPlayerFilter disabled (-> use configured default values) + { + msgpack_validator_filter_settings.msgpack_validator_layer_filter = layer_active_vector; + } + + // Example: sopas.FREchoFilter = "1", sopas.LFPangleRangeFilter = "0 -180 180 -90.0002 90.0002 1", sopas.LFPlayerFilter = "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1" + // msgpack_validator_required_echos = { 0 }, msgpack_validator_angles = { -3.14159 3.14159 -1.5708 1.5708 } [rad], msgpack_validator_layer_filter = { 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 } + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): sopas.FREchoFilter = \"" << host_FREchoFilter + << "\", sopas.LFPangleRangeFilter = \"" << host_LFPangleRangeFilter + << "\", sopas.LFPlayerFilter = \"" << host_LFPlayerFilter << "\""); + ROS_INFO_STREAM("SickScanServices::queryMultiScanFiltersettings(): msgpack_validator_required_echos = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_required_echos) + << " }, msgpack_validator_angles = { " << msgpack_validator_filter_settings.msgpack_validator_azimuth_start << " " << msgpack_validator_filter_settings.msgpack_validator_azimuth_end + << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_start << " " << msgpack_validator_filter_settings.msgpack_validator_elevation_end + << " } [rad], msgpack_validator_layer_filter = { " << sick_scansegment_xd::util::printVector(msgpack_validator_filter_settings.msgpack_validator_layer_filter) << " }"); + return true; +} +#endif // SCANSEGMENT_XD_SUPPORT + +#if defined SCANSEGMENT_XD_SUPPORT && SCANSEGMENT_XD_SUPPORT > 0 +/*! +* Sends the SOPAS command to write multiScan136 filter settings (FREchoFilter, LFPangleRangeFilter, host_LFPlayerFilter) +* @param[in] host_FREchoFilter FREchoFilter settings, default: 1, otherwise 0 for FIRST_ECHO (EchoCount=1), 1 for ALL_ECHOS (EchoCount=3), or 2 for LAST_ECHO (EchoCount=1) +* @param[in] host_LFPangleRangeFilter LFPangleRangeFilter settings, default: "0 -180.0 +180.0 -90.0 +90.0 1", otherwise " " with azimuth and elevation given in degree +* @param[in] host_LFPlayerFilter LFPlayerFilter settings, default: "0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1", otherwise " ... " with 1 for enabled and 0 for disabled +* @param[in] host_LFPintervalFilter Optionally set LFPintervalFilter to " " with 1 for enabled and 0 for disabled and N to reduce output to every N-th scan +*/ +bool sick_scan_xd::SickScanServices::writeMultiScanFiltersettings(int host_FREchoFilter, const std::string& host_LFPangleRangeFilter, const std::string& host_LFPlayerFilter, const std::string& host_LFPintervalFilter, const std::string& scanner_type) + +{ + bool enableFREchoFilter = true, enableLFPangleRangeFilter = true, enableLFPlayerFilter = true, enableLFPintervalFilter = true; + if (scanner_type == SICK_SCANNER_PICOSCAN_NAME) // LFPangleRangeFilter, LFPlayerFilter ant LFPintervalFilter not supported by picoscan150 + { + // enableLFPangleRangeFilter = false; + enableLFPlayerFilter = false; + // enableLFPintervalFilter = false; + } + + // Write FREchoFilter + if(enableFREchoFilter && host_FREchoFilter >= 0) // otherwise not configured or supported + { + std::string sopasRequest = "sWN FREchoFilter " + std::to_string(host_FREchoFilter), sopasExpectedResponse = "sWA FREchoFilter"; + if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + return false; + } + } + + // Write LFPangleRangeFilter + if(enableLFPangleRangeFilter && !host_LFPangleRangeFilter.empty()) // otherwise not configured or supported + { + // convert deg to rad and float to hex + std::vector parameter_token; + sick_scansegment_xd::util::parseVector(host_LFPangleRangeFilter, parameter_token, ' '); + if(parameter_token.size() != 6) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): can't split host_LFPangleRangeFilter = \"" << host_LFPangleRangeFilter << "\", expected 6 values separated by space"); + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings() failed."); + return false; + } + int filter_enabled = std::stoi(parameter_token[0]); // + std::vector angle_deg; + for(int n = 1; n < 5; n++) + angle_deg.push_back(std::stof(parameter_token[n])); + int beam_increment = std::stoi(parameter_token[5]); // + std::stringstream sopas_parameter; + sopas_parameter << filter_enabled; + for(int n = 0; n < angle_deg.size(); n++) + { + // sopas_parameter << " " << convertFloatToHexString(angle_deg[n] * M_PI / 180, SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); + sopas_parameter << " " << convertAngleDegToHexString(angle_deg[n], SCANSEGMENT_XD_SOPAS_ARGS_BIG_ENDIAN); + } + sopas_parameter << " " << beam_increment; + // Write LFPangleRangeFilter + std::string sopasRequest = "sWN LFPangleRangeFilter " + sopas_parameter.str(), sopasExpectedResponse = "sWA LFPangleRangeFilter"; + if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + return false; + } + } + + // Write LFPlayerFilter + if(enableLFPlayerFilter && !host_LFPlayerFilter.empty()) // otherwise not configured or supported + { + std::string sopasRequest = "sWN LFPlayerFilter " + host_LFPlayerFilter, sopasExpectedResponse = "sWA LFPlayerFilter"; + if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + return false; + } + } + + // Write LFPintervalFilter + if(enableLFPintervalFilter && !host_LFPintervalFilter.empty()) // otherwise not configured or supported + { + std::string sopasRequest = "sWN LFPintervalFilter " + host_LFPintervalFilter, sopasExpectedResponse = "sWA LFPintervalFilter"; + if (!sendSopasCmdCheckResponse(sopasRequest, sopasExpectedResponse)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"" << sopasRequest << "\") failed."); + return false; + } + } + + // Apply the settings + if (!sendSopasCmdCheckResponse("sMN Run", "sAN Run")) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::writeMultiScanFiltersettings(): sendSopasCmdCheckResponse(\"sMN Run\") failed."); + return false; + } + return true; +} +#endif // SCANSEGMENT_XD_SUPPORT + +/*! +* Callbacks for service messages. +* @param[in] service_request ros service request to lidar +* @param[out] service_response service response from lidar +* @return true on success, false in case of errors. +*/ +bool sick_scan_xd::SickScanServices::serviceCbSCdevicestate(sick_scan_srv::SCdevicestateSrv::Request &service_request, sick_scan_srv::SCdevicestateSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sRN SCdevicestate"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + service_response.state = 2; // ERROR + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + std::string response_str((char*)sopasReplyBin.data(), sopasReplyBin.size()); + std::size_t state_pos = response_str.find("SCdevicestate"); + if (state_pos != std::string::npos && state_pos + 14 < sopasReplyBin.size()) + { + uint8_t state_byte = sopasReplyBin[state_pos + 14]; + if(state_byte >= '0') + state_byte -= '0'; + service_response.state = state_byte; + } + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\" = \"" << DataDumper::binDataToAsciiString(sopasReplyBin.data(), sopasReplyBin.size()) << "\""); + + return true; +} + +bool sick_scan_xd::SickScanServices::serviceCbSCreboot(sick_scan_srv::SCrebootSrv::Request &service_request, sick_scan_srv::SCrebootSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sMN mSCreboot"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + if(!sendAuthorization()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); + return false; + } + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + if(!sendRun()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + return true; +} + +bool sick_scan_xd::SickScanServices::serviceCbSCsoftreset(sick_scan_srv::SCsoftresetSrv::Request &service_request, sick_scan_srv::SCsoftresetSrv::Response &service_response) +{ + std::string sopasCmd = std::string("sMN mSCsoftreset"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + if(!sendAuthorization()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); + return false; + } + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + if(!sendRun()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); + return false; + } + service_response.success = true; + + return true; +} + +bool sick_scan_xd::SickScanServices::serviceCbSickScanExit(sick_scan_srv::SickScanExitSrv::Request &service_request, sick_scan_srv::SickScanExitSrv::Response &service_response) +{ + /* + std::string sopasCmd = std::string("sMN mSCsoftreset"); + std::vector sopasReplyBin; + std::string sopasReplyString; + + service_response.success = false; + if(!sendAuthorization()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendAuthorization failed for command\"" << sopasCmd << "\""); + return false; + } + + if(!sendSopasAndCheckAnswer(sopasCmd, sopasReplyBin, sopasReplyString)) + { + ROS_ERROR_STREAM("## ERROR SickScanServices::sendSopasAndCheckAnswer failed on sending command\"" << sopasCmd << "\""); + return false; + } + + ROS_INFO_STREAM("SickScanServices: request: \"" << sopasCmd << "\""); + ROS_INFO_STREAM("SickScanServices: response: \"" << sopasReplyString << "\""); + + if(!sendRun()) + { + ROS_ERROR_STREAM("## ERROR SickScanServices: sendRun failed for command\"" << sopasCmd << "\""); + return false; + } + */ + service_response.success = stopScannerAndExit(false); + return true; +} + +#if __ROS_VERSION > 0 +bool sick_scan_xd::SickScanServices::serviceCbFieldSetRead(sick_scan_srv::FieldSetReadSrv::Request &service_request, sick_scan_srv::FieldSetReadSrv::Response &service_response) +{ + SickScanFieldMonSingleton *fieldMon = SickScanFieldMonSingleton::getInstance(); + int field_set_selection_method = fieldMon->getFieldSelectionMethod(); + int active_field_set = fieldMon->getActiveFieldset(); + std::vector sopasReply; + int result1 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); + int result2 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); + service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess); + service_response.field_set_selection_method = field_set_selection_method; + service_response.active_field_set = active_field_set; + return true; +} + +bool sick_scan_xd::SickScanServices::serviceCbFieldSetWrite(sick_scan_srv::FieldSetWriteSrv::Request &service_request, sick_scan_srv::FieldSetWriteSrv::Response &service_response) +{ + int field_set_selection_method = service_request.field_set_selection_method_in; + int active_field_set = service_request.active_field_set_in; + std::vector sopasReply; + std::string sopasReplyString; + int result1 = ExitSuccess, result2 = ExitSuccess; + if (field_set_selection_method >= 0) + { + if(!sendAuthorization()) + result1 = ExitError; + if(!sendSopasAndCheckAnswer("sMN LMCstopmeas", sopasReply, sopasReplyString)) + result1 = ExitError; + result1 = m_common_tcp->writeFieldSetSelectionMethod(field_set_selection_method, sopasReply); + if(!sendSopasAndCheckAnswer("sMN LMCstartmeas", sopasReply, sopasReplyString)) + result1 = ExitError; + if(!sendSopasAndCheckAnswer("sMN Run", sopasReply, sopasReplyString)) + result1 = ExitError; + } + if (active_field_set >= 0) + { + result2 = m_common_tcp->writeActiveFieldSet(active_field_set, sopasReply); + } + int result3 = m_common_tcp->readFieldSetSelectionMethod(field_set_selection_method, sopasReply); + int result4 = m_common_tcp->readActiveFieldSet(active_field_set, sopasReply); + service_response.success = (result1 == ExitSuccess && result2 == ExitSuccess && result3 == ExitSuccess && result4 == ExitSuccess); + service_response.field_set_selection_method = field_set_selection_method; + service_response.active_field_set = active_field_set; + service_response.success = true; + return true; +} +#endif diff --git a/include/sick_scan/sick_generic_field_mon.h b/include/sick_scan/sick_generic_field_mon.h index 51915d0..8e3dcef 100644 --- a/include/sick_scan/sick_generic_field_mon.h +++ b/include/sick_scan/sick_generic_field_mon.h @@ -1,212 +1,212 @@ -#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ -/* - * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning, Hildesheim - * Copyright (C) 2018, SICK AG, Waldkirch - * All rights reserved. - * -* 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. -* -* -* 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 Osnabrueck University nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* * Neither the name of SICK AG nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission -* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. - * - * Created on: 28th May 2018 - * - * Authors: - * Michael Lehning - * - */ - -#ifndef SICK_GENERIC_FIELD_MON_H_ -#define SICK_GENERIC_FIELD_MON_H_ - -#include -#include -#include -#include -#include - -#include - -namespace sick_scan_xd -{ - enum SickScanMonFieldType - { - MON_FIELD_RADIAL = 0, // not supported - MON_FIELD_RECTANGLE = 1, - MON_FIELD_SEGMENTED = 2, - MON_FIELD_DYNAMIC = 3 - }; - - class SickScanMonFieldConverter - { - public: - /* - ** @brief Converts a point of a segmented field to carthesian coordinates - ** @param[in] range range in meter - ** @param[in] angle_rad angle in radians - ** @param[out] x x in meter in ros coordinate system - ** @param[out] y y in meter in ros coordinate system - */ - static void segmentedFieldPointToCarthesian(float range, float angle_rad, float& x, float& y); - - /* - ** @brief Converts a rectangular field to carthesian coordinates, i.e. to 4 corner points carthesian (ros) coordinates - ** @param[in] distRefPointMeter range of ref point (rect center) in meter - ** @param[in] angleRefPointRad angle of ref point (rect center) in radians - ** @param[in] rotAngleRad rotation of rectangle in radians - ** @param[in] rectWidthMeter width of rectangle in meter - ** @param[in] rectLengthMeter width of rectangle in meter - ** @param[out] points_x x of corner points in meter in ros coordinate system - ** @param[out] points_y y of corner points in meter in ros coordinate system - */ - static void rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4]); - - /* - ** @brief Converts a dynamic field to carthesian coordinates. The dynamic field is just converted into 2 rectangular fields, - ** each rectangular field described by to 4 corner points carthesian (ros) coordinates. - ** The first rectangular field has size rectWidthMeter x maxLengthMeter, the second rectangular field has size rectWidthMeter x rectLengthMeter. - ** The rectangular fields are returned by 4 corner points (points_x[n], points_y[n]) with 0 <= n < 4 for the first (big) rectangle and 4 <= n < 8 for the second (smaller) rectangle. - ** @param[in] distRefPointMeter range of ref point (rect center) in meter - ** @param[in] angleRefPointRad angle of ref point (rect center) in radians - ** @param[in] rotAngleRad rotation of rectangle in radians - ** @param[in] rectWidthMeter width of rectangle in meter - ** @param[in] rectLengthMeter width of rectangle in meter at v = 0 - ** @param[in] maxSpeedMeterPerSec max speed (unused) - ** @param[in] maxLengthMeter width of rectangle in meter at v = max. speed - ** @param[out] points_x x of corner points in meter in ros coordinate system - ** @param[out] points_y y of corner points in meter in ros coordinate system - */ - static void dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8]); - - }; - - class SickScanMonField - { - public: - - SickScanMonFieldType& fieldType(void) { return m_fieldType; } - const SickScanMonFieldType& fieldType(void) const { return m_fieldType; } - - void pushFieldPointCarthesian(float x, float y) - { - m_fieldPoints_X.push_back(x); - m_fieldPoints_Y.push_back(y); - } - - int getPointCount(void) const - { - assert(m_fieldPoints_X.size() == m_fieldPoints_Y.size()); - return (int)m_fieldPoints_X.size(); - } - - const std::vector& getFieldPointsX(void) const { return m_fieldPoints_X; } - const std::vector& getFieldPointsY(void) const { return m_fieldPoints_Y; } - - private: - SickScanMonFieldType m_fieldType = MON_FIELD_RADIAL; - std::vector m_fieldPoints_X; - std::vector m_fieldPoints_Y; - }; - - - class SickScanFieldMonSingleton - { - private: - /* Here will be the instance stored. */ - static SickScanFieldMonSingleton *instance; - - /* Private constructor to prevent instancing. */ - SickScanFieldMonSingleton(); - - std::vectormonFields; - int active_mon_fieldset = 0; - int mon_field_selection_method = 0; // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" - - public: - /* Static access method. */ - static SickScanFieldMonSingleton *getInstance(); - - const std::vector& getMonFields(void) const { return monFields; } - - void setActiveFieldset(int active_fieldset) { active_mon_fieldset = active_fieldset; } - int getActiveFieldset(void) { return active_mon_fieldset; } - - void setFieldSelectionMethod(int field_selection_method) { mon_field_selection_method = field_selection_method; } // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" - int getFieldSelectionMethod(void) { return mon_field_selection_method; } - - int parseAsciiLIDinputstateMsg(unsigned char* datagram, int datagram_length); - int parseBinaryLIDinputstateMsg(unsigned char* datagram, int datagram_length, sick_scan_msg::LIDinputstateMsg& inputstate_msg); - - void parseActiveFieldSetResponse(unsigned char* datagram, int datagram_length, uint16_t* active_field_set); - void parseFieldSetSelectionMethodResponse(unsigned char* datagram, int datagram_length, uint8_t* field_set_selection_method); - - int parseBinaryDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); - - int parseAsciiDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); - - std::string LIDinputstateMsgToString(const sick_scan_msg::LIDinputstateMsg& inputstate_msg); - }; - - -#if 0 - class SickScanRadar - { - public: - SickScanRadar(SickScanCommon *commonPtr_) - { - commonPtr = commonPtr_; - } - void setEmulation(bool _emul); - bool getEmulation(void); - int parseDatagram(rosTime timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol); - int parseAsciiDatagram(char* datagram, size_t datagram_length, sick_scan_msg::RadarScan *msgPtr, std::vector &objectList, std::vector &rawTargetList); /* , SickScanConfig &config, */ // sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask); - void simulateAsciiDatagram(unsigned char * receiveBuffer, int* actual_length); - private: -// SickScanCommon *commonPtr; - void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern); - bool emul; - }; -#endif - -} /* namespace sick_scan_xd */ -#endif // SICK_GENERIC_RADAR_H_ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +/* + * Copyright (C) 2018, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2018, SICK AG, Waldkirch + * All rights reserved. + * +* 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. +* +* +* 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 Osnabrueck University nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* * Neither the name of SICK AG nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission +* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. + * + * Created on: 28th May 2018 + * + * Authors: + * Michael Lehning + * + */ + +#ifndef SICK_GENERIC_FIELD_MON_H_ +#define SICK_GENERIC_FIELD_MON_H_ + +#include +#include +#include +#include +#include + +#include + +namespace sick_scan_xd +{ + enum SickScanMonFieldType + { + MON_FIELD_RADIAL = 0, // not supported + MON_FIELD_RECTANGLE = 1, + MON_FIELD_SEGMENTED = 2, + MON_FIELD_DYNAMIC = 3 + }; + + class SickScanMonFieldConverter + { + public: + /* + ** @brief Converts a point of a segmented field to carthesian coordinates + ** @param[in] range range in meter + ** @param[in] angle_rad angle in radians + ** @param[out] x x in meter in ros coordinate system + ** @param[out] y y in meter in ros coordinate system + */ + static void segmentedFieldPointToCarthesian(float range, float angle_rad, float& x, float& y); + + /* + ** @brief Converts a rectangular field to carthesian coordinates, i.e. to 4 corner points carthesian (ros) coordinates + ** @param[in] distRefPointMeter range of ref point (rect center) in meter + ** @param[in] angleRefPointRad angle of ref point (rect center) in radians + ** @param[in] rotAngleRad rotation of rectangle in radians + ** @param[in] rectWidthMeter width of rectangle in meter + ** @param[in] rectLengthMeter width of rectangle in meter + ** @param[out] points_x x of corner points in meter in ros coordinate system + ** @param[out] points_y y of corner points in meter in ros coordinate system + */ + static void rectangularFieldToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float points_x[4], float points_y[4]); + + /* + ** @brief Converts a dynamic field to carthesian coordinates. The dynamic field is just converted into 2 rectangular fields, + ** each rectangular field described by to 4 corner points carthesian (ros) coordinates. + ** The first rectangular field has size rectWidthMeter x maxLengthMeter, the second rectangular field has size rectWidthMeter x rectLengthMeter. + ** The rectangular fields are returned by 4 corner points (points_x[n], points_y[n]) with 0 <= n < 4 for the first (big) rectangle and 4 <= n < 8 for the second (smaller) rectangle. + ** @param[in] distRefPointMeter range of ref point (rect center) in meter + ** @param[in] angleRefPointRad angle of ref point (rect center) in radians + ** @param[in] rotAngleRad rotation of rectangle in radians + ** @param[in] rectWidthMeter width of rectangle in meter + ** @param[in] rectLengthMeter width of rectangle in meter at v = 0 + ** @param[in] maxSpeedMeterPerSec max speed (unused) + ** @param[in] maxLengthMeter width of rectangle in meter at v = max. speed + ** @param[out] points_x x of corner points in meter in ros coordinate system + ** @param[out] points_y y of corner points in meter in ros coordinate system + */ + static void dynamicFieldPointToCarthesian(float distRefPointMeter, float angleRefPointRad, float rotAngleRad, float rectWidthMeter, float rectLengthMeter, float maxSpeedMeterPerSec, float maxLengthMeter, float points_x[8], float points_y[8]); + + }; + + class SickScanMonField + { + public: + + SickScanMonFieldType& fieldType(void) { return m_fieldType; } + const SickScanMonFieldType& fieldType(void) const { return m_fieldType; } + + void pushFieldPointCarthesian(float x, float y) + { + m_fieldPoints_X.push_back(x); + m_fieldPoints_Y.push_back(y); + } + + int getPointCount(void) const + { + assert(m_fieldPoints_X.size() == m_fieldPoints_Y.size()); + return (int)m_fieldPoints_X.size(); + } + + const std::vector& getFieldPointsX(void) const { return m_fieldPoints_X; } + const std::vector& getFieldPointsY(void) const { return m_fieldPoints_Y; } + + private: + SickScanMonFieldType m_fieldType = MON_FIELD_RADIAL; + std::vector m_fieldPoints_X; + std::vector m_fieldPoints_Y; + }; + + + class SickScanFieldMonSingleton + { + private: + /* Here will be the instance stored. */ + static SickScanFieldMonSingleton *instance; + + /* Private constructor to prevent instancing. */ + SickScanFieldMonSingleton(); + + std::vectormonFields; + int active_mon_fieldset = 0; + int mon_field_selection_method = 0; // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" + + public: + /* Static access method. */ + static SickScanFieldMonSingleton *getInstance(); + + const std::vector& getMonFields(void) const { return monFields; } + + void setActiveFieldset(int active_fieldset) { active_mon_fieldset = active_fieldset; } // asssumes 1 <= active_fieldset <= max active fieldsets + int getActiveFieldset(void) { return active_mon_fieldset; } // asssumes 1 <= active_fieldset <= max active fieldsets + + void setFieldSelectionMethod(int field_selection_method) { mon_field_selection_method = field_selection_method; } // FieldSetSelectionMethod: 0 = digital inputs (default), 1 = telegram "sWN ActiveFieldSet" + int getFieldSelectionMethod(void) { return mon_field_selection_method; } + + int parseAsciiLIDinputstateMsg(unsigned char* datagram, int datagram_length); + int parseBinaryLIDinputstateMsg(unsigned char* datagram, int datagram_length, sick_scan_msg::LIDinputstateMsg& inputstate_msg); + + void parseActiveFieldSetResponse(unsigned char* datagram, int datagram_length, uint16_t* active_field_set); + void parseFieldSetSelectionMethodResponse(unsigned char* datagram, int datagram_length, uint8_t* field_set_selection_method); + + int parseBinaryDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); + + int parseAsciiDatagram(std::vector datagramm, float rectFieldAngleRefPointOffsetRad); + + std::string LIDinputstateMsgToString(const sick_scan_msg::LIDinputstateMsg& inputstate_msg); + }; + + +#if 0 + class SickScanRadar + { + public: + SickScanRadar(SickScanCommon *commonPtr_) + { + commonPtr = commonPtr_; + } + void setEmulation(bool _emul); + bool getEmulation(void); + int parseDatagram(rosTime timeStamp, unsigned char *receiveBuffer, int actual_length, bool useBinaryProtocol); + int parseAsciiDatagram(char* datagram, size_t datagram_length, sick_scan_msg::RadarScan *msgPtr, std::vector &objectList, std::vector &rawTargetList); /* , SickScanConfig &config, */ // sensor_msgs::LaserScan &msg, int &numEchos, int &echoMask); + void simulateAsciiDatagram(unsigned char * receiveBuffer, int* actual_length); + private: +// SickScanCommon *commonPtr; + void simulateAsciiDatagramFromFile(unsigned char *receiveBuffer, int *actual_length, std::string filePattern); + bool emul; + }; +#endif + +} /* namespace sick_scan_xd */ +#endif // SICK_GENERIC_RADAR_H_ diff --git a/include/sick_scan/sick_scan_marker.h b/include/sick_scan/sick_scan_marker.h index bed02b9..46378d9 100644 --- a/include/sick_scan/sick_scan_marker.h +++ b/include/sick_scan/sick_scan_marker.h @@ -1,123 +1,123 @@ -#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ -/* - * @brief Implementation of object markers for sick_scan - * - * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim - * Copyright (C) 2021, SICK AG, Waldkirch - * All rights reserved. - * -* 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. -* -* -* 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 Osnabrueck University nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* * Neither the name of SICK AG nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission -* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. - * - * Created on: 13.01.2021 - * - * Authors: - * Michael Lehning - * - */ - -#ifndef SICK_SCAN_MARKER_H_ -#define SICK_SCAN_MARKER_H_ - -#include -#include -#include "sick_scan/sick_range_filter.h" -#include "sick_scan/sick_generic_field_mon.h" - - -namespace sick_scan_xd -{ - class SickScanMarker - { - public: - - SickScanMarker(rosNodePtr nh = 0, const std::string & marker_topic = "", const std::string & marker_frame_id = ""); - - virtual ~SickScanMarker(); - - void updateMarker(const std::vector& fields, int fieldset, int eval_field_logic); - - void updateMarker(sick_scan_msg::LIDinputstateMsg& msg, int eval_field_logic); - - void updateMarker(sick_scan_msg::LIDoutputstateMsg& msg, int eval_field_logic); - - void updateMarker(sick_scan_msg::LFErecMsg& msg, int eval_field_logic); - - protected: - - class FieldInfo - { - public: - FieldInfo(int idx=0, int result=0, const std::string& status="", const std::string& name="", const ros_std_msgs::ColorRGBA& color= ros_std_msgs::ColorRGBA()) - : field_index_scan_mon(idx), field_result(result), field_status(status), field_name(name), field_color(color) {} - int field_index_scan_mon; // 0 to 47 - int field_result;// 0 = invalid = gray, 1 = free/clear = green, 2 = infringed = yellow - std::string field_status; // field_result as string - std::string field_name; // name within the field set ("1", "2" or "3") - ros_std_msgs::ColorRGBA field_color; // field_result as color - }; - - void publishMarker(void); - std::vector createMonFieldMarker(const std::vector& field_info); - std::vector createMonFieldLegend(const std::vector& field_info); - std::vector createMonFieldsetLegend(int fieldset); - std::vector createOutputStateLegend(const std::vector& output_state, const std::vector& output_count, const std::vector& output_colors); - - rosNodePtr m_nh; - std::string m_frame_id; - rosPublisher m_marker_publisher; - int m_scan_mon_fieldset; - std::vector m_scan_mon_fields; - std::vector m_scan_mon_field_marker; - std::vector m_scan_mon_field_legend; - std::vector m_scan_fieldset_legend; - std::vector m_scan_outputstate_legend; - double m_marker_output_legend_offset_x; - sick_scan_xd::SickCloudTransform m_add_transform_xyz_rpy; // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) - - }; /* class SickScanMarker */ - -} /* namespace sick_scan_xd */ -#endif /* SICK_SCAN_MARKER_H_ */ +#include "sick_scan/sick_scan_base.h" /* Base definitions included in all header files, added by add_sick_scan_base_header.py. Do not edit this line. */ +/* + * @brief Implementation of object markers for sick_scan + * + * Copyright (C) 2021, Ing.-Buero Dr. Michael Lehning, Hildesheim + * Copyright (C) 2021, SICK AG, Waldkirch + * All rights reserved. + * +* 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. +* +* +* 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 Osnabrueck University nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* * Neither the name of SICK AG nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission +* * Neither the name of Ing.-Buero Dr. Michael Lehning 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. + * + * Created on: 13.01.2021 + * + * Authors: + * Michael Lehning + * + */ + +#ifndef SICK_SCAN_MARKER_H_ +#define SICK_SCAN_MARKER_H_ + +#include +#include +#include "sick_scan/sick_range_filter.h" +#include "sick_scan/sick_generic_field_mon.h" + + +namespace sick_scan_xd +{ + class SickScanMarker + { + public: + + SickScanMarker(rosNodePtr nh = 0, const std::string & marker_topic = "", const std::string & marker_frame_id = ""); + + virtual ~SickScanMarker(); + + void updateMarker(const std::vector& fields, int fieldset, int eval_field_logic); + + void updateMarker(sick_scan_msg::LIDinputstateMsg& msg, int eval_field_logic); + + void updateMarker(sick_scan_msg::LIDoutputstateMsg& msg, int eval_field_logic); + + void updateMarker(sick_scan_msg::LFErecMsg& msg, int eval_field_logic); + + protected: + + class FieldInfo + { + public: + FieldInfo(int idx=0, int result=0, const std::string& status="", const std::string& name="", const ros_std_msgs::ColorRGBA& color= ros_std_msgs::ColorRGBA()) + : field_index_scan_mon(idx), field_result(result), field_status(status), field_name(name), field_color(color) {} + int field_index_scan_mon; // 0 to 47 + int field_result;// 0 = invalid = gray, 1 = free/clear = green, 2 = infringed = yellow + std::string field_status; // field_result as string + std::string field_name; // name within the field set ("1", "2" or "3") + ros_std_msgs::ColorRGBA field_color; // field_result as color + }; + + void publishMarker(void); + std::vector createMonFieldMarker(const std::vector& field_info); + std::vector createMonFieldLegend(const std::vector& field_info); + std::vector createMonFieldsetLegend(int fieldset); + std::vector createOutputStateLegend(const std::vector& output_state, const std::vector& output_count, const std::vector& output_colors); + + rosNodePtr m_nh; + std::string m_frame_id; + rosPublisher m_marker_publisher; + int m_scan_mon_fieldset; // active fieldset assuming m_scan_mon_fieldset >= 1 (default: 1) + std::vector m_scan_mon_fields; + std::vector m_scan_mon_field_marker; + std::vector m_scan_mon_field_legend; + std::vector m_scan_fieldset_legend; + std::vector m_scan_outputstate_legend; + double m_marker_output_legend_offset_x; + sick_scan_xd::SickCloudTransform m_add_transform_xyz_rpy; // Apply an additional transform to the cartesian pointcloud, default: "0,0,0,0,0,0" (i.e. no transform) + + }; /* class SickScanMarker */ + +} /* namespace sick_scan_xd */ +#endif /* SICK_SCAN_MARKER_H_ */