From 13bcc5562b701f471a29d2d2234e177e4c2e5cb9 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Sat, 4 Jul 2020 00:17:48 +0200
Subject: [PATCH] add safety zone spawner node, improve and rename config file

---
 CMakeLists.txt                       | 92 +++++++++++++++-------------
 LICENSE                              |  2 +-
 config/config.yaml                   | 21 +++++++
 config/mqtt.yaml                     |  7 ---
 launch/simulation_rosrag.launch      |  6 +-
 launch/simulation_rosrag_test.launch |  8 ++-
 mainpage.dox                         | 28 +++++++++
 package.xml                          |  1 +
 src/safety_zone_spawner.cpp          | 87 ++++++++++++++++++++++++++
 9 files changed, 196 insertions(+), 56 deletions(-)
 create mode 100644 config/config.yaml
 delete mode 100644 config/mqtt.yaml
 create mode 100644 mainpage.dox
 create mode 100644 src/safety_zone_spawner.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 69b371c..4d0ceb9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,31 +1,32 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0.2)
 project(panda_mqtt_connector)
 
-add_compile_options(-std=c++14)
 set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS} -std=c++14")
 
 # Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
 # catkin packages
-find_package(catkin REQUIRED
-             COMPONENTS controller_manager
-                        effort_controllers
-                        gazebo_ros
-                        genmsg
-                        joint_state_controller
-                        joint_state_publisher
-                        joint_trajectory_controller
-                        robot_state_publisher
-                        roscpp
-                        std_msgs
-                        tf
-                        xacro
-                        moveit_core
-                        moveit_visual_tools
-                        moveit_ros_planning
-                        moveit_ros_planning_interface
-                        controller_interface
-                        hardware_interface)
+find_package(catkin REQUIRED COMPONENTS
+        controller_manager
+        effort_controllers
+        gazebo_ros
+        genmsg
+        joint_state_controller
+        joint_state_publisher
+        joint_trajectory_controller
+        robot_state_publisher
+        roscpp
+        std_msgs
+        tf
+        xacro
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        simulation_util
+        )
 
 # System dependencies are found with CMake's conventions
 find_package(Boost REQUIRED COMPONENTS filesystem)
@@ -33,14 +34,12 @@ find_package(Boost REQUIRED COMPONENTS filesystem)
 find_package(Protobuf 3 REQUIRED)
 include_directories(${Protobuf_INCLUDE_DIRS})
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
-protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/linkstate.proto)
+#protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/robotconfig.proto)
 protobuf_generate_cpp(PROTO_SRCS PROTO_HDRS proto/dataconfig.proto proto/linkstate.proto proto/robotconfig.proto proto/trajectory.proto)
 
 SET_SOURCE_FILES_PROPERTIES(${PROTO_SRC} ${PROTO_HDRS} PROPERTIES GENERATED TRUE)
-message("psource " ${PROTO_SRC})
-message("pheaders" ${PROTO_HDRS})
 
 find_package(PkgConfig REQUIRED)
 
@@ -52,25 +51,20 @@ set(PAHO_MQTT_C_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/lib/paho.mqtt.c/src")
 
 add_subdirectory(lib/paho.mqtt.cpp)
 
-get_cmake_property(_variableNames VARIABLES)
-list (SORT _variableNames)
-foreach (_variableName ${_variableNames})
-    message(STATUS "${_variableName}=${${_variableName}}")
-endforeach()
-
 # ################################################################################################################################
 # catkin specific configuration ##
 # ################################################################################################################################
 # The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
 catkin_package(CATKIN_DEPENDS
-               moveit_core
-               moveit_visual_tools
-               moveit_ros_planning_interface
-               controller_interface
-               hardware_interface
-               pluginlib
-               DEPENDS
-              # system_lib
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        pluginlib
+        simulation_util
+        DEPENDS
+        # system_lib
         )
 
 # ################################################################################################################################
@@ -79,7 +73,6 @@ catkin_package(CATKIN_DEPENDS
 
 # Specify additional locations of header files Your package locations should be listed before other locations
 include_directories(
-        include
         lib/paho.mqtt.c/src
         lib/paho.mqtt.cpp/src
         ${catkin_INCLUDE_DIRS}
@@ -90,14 +83,23 @@ add_dependencies(paho-mqttpp3 paho-mqtt3c)
 add_library(TrajectoryUtil src/util/TrajectoryUtil.cpp src/util/TrajectoryUtil.h)
 add_library(MqttUtil src/util/MqttUtil.cpp src/util/MqttUtil.h)
 
-# Specify libraries to link a library or executable target against
-target_link_libraries(TrajectoryUtil ${catkin_LIBRARIES})
 
 add_executable(RobotStateProvider src/RobotStateProvider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(TimedPlanner src/TimedPlanner.cpp)
 add_executable(MqttToRosNode src/MqttToRosNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
 add_executable(MqttRosConnectionTestNode src/MqttRosConnectionTestNode.cpp ${PROTO_SRCS} ${PROTO_HDRS})
+add_executable(${PROJECT_NAME}_safety_zone_spawner src/safety_zone_spawner.cpp)
+
+
+set_target_properties(${PROJECT_NAME}_safety_zone_spawner PROPERTIES OUTPUT_NAME safety_zone_spawner PREFIX "")
+
 
+add_dependencies(${PROJECT_NAME}_safety_zone_spawner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(TrajectoryUtil
+        ${catkin_LIBRARIES}
+        )
 target_link_libraries(RobotStateProvider LINK_PUBLIC
         MqttUtil
         ${catkin_LIBRARIES}
@@ -117,3 +119,5 @@ target_link_libraries(MqttRosConnectionTestNode LINK_PUBLIC
         ${Protobuf_LIBRARIES}
         )
 target_link_libraries(TimedPlanner ${catkin_LIBRARIES} TrajectoryUtil)
+
+target_link_libraries(${PROJECT_NAME}_safety_zone_spawner ${catkin_LIBRARIES})
diff --git a/LICENSE b/LICENSE
index b591282..a943dd9 100644
--- a/LICENSE
+++ b/LICENSE
@@ -1,4 +1,4 @@
-Copyright 2019 Erdal Pekel
+Copyright 2020 Technische Universität Dresden, Software Technology Group
 
 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
 
diff --git a/config/config.yaml b/config/config.yaml
new file mode 100644
index 0000000..e891d24
--- /dev/null
+++ b/config/config.yaml
@@ -0,0 +1,21 @@
+panda_mqtt_connector:
+  server: "tcp://localhost:1883"
+  topics:
+  - robotConfig: "robotconfig"
+  - dataConfig: "dataconfig"
+  zone_size: 0.5
+  zones:
+    - "1 1"
+    - "-1 -1 1"
+  joints:
+    - panda/Joint0: "panda::panda_link0"
+    - panda/Joint1: "panda::panda_link1"
+    - panda/Joint2: "panda::panda_link2"
+    - panda/Joint3: "panda::panda_link3"
+    - panda/Joint4: "panda::panda_link4"
+    - panda/Joint5: "panda::panda_link5"
+    - panda/Joint6: "panda::panda_link6"
+    - panda/LeftFinger: "panda::panda_leftfinger"
+    - panda/RightFinger: "panda::panda_rightfinger"
+  end_effectors:
+    - panda/EndEffector: "panda::panda_link7"
\ No newline at end of file
diff --git a/config/mqtt.yaml b/config/mqtt.yaml
deleted file mode 100644
index ba53262..0000000
--- a/config/mqtt.yaml
+++ /dev/null
@@ -1,7 +0,0 @@
-panda_mqtt_connector:
-  server: "tcp://localhost:1883"
-safety_zone_spawner:
-  size: 0.5
-  zones:
-    - "1 1"
-    - "-1 -1 1"
\ No newline at end of file
diff --git a/launch/simulation_rosrag.launch b/launch/simulation_rosrag.launch
index 65e66c6..9c55f39 100644
--- a/launch/simulation_rosrag.launch
+++ b/launch/simulation_rosrag.launch
@@ -1,13 +1,13 @@
 <launch>
-    <arg name="mqtt_server" default="" />
+    <arg name="mqtt_server" default=""/>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
     <!-- the launch file parameter mqtt_server overrides the config file parameter -->
-    <rosparam file="$(find panda_mqtt_connector)/config/mqtt.yaml" command="load" />
+    <rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
     <rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
 
     <node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
     <node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
     <node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
-    <node name="gazebo_zones" pkg="gazebo_zones" type="gazebo_zones" respawn="false" output="screen"/>
+    <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
 </launch>
diff --git a/launch/simulation_rosrag_test.launch b/launch/simulation_rosrag_test.launch
index c489c2d..70a4b67 100644
--- a/launch/simulation_rosrag_test.launch
+++ b/launch/simulation_rosrag_test.launch
@@ -1,8 +1,14 @@
 <launch>
+    <arg name="mqtt_server" default=""/>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
+    <!-- the launch file parameter mqtt_server overrides the config file parameter -->
+    <rosparam file="$(find panda_mqtt_connector)/config/config.yaml" command="load" />
+    <rosparam unless="$(eval arg('mqtt_server') == '')" param="panda_mqtt_connector/server" subst_value="True">$(arg mqtt_server)</rosparam>
+
     <node name="TimedPlanner" pkg="panda_mqtt_connector" type="TimedPlanner" respawn="false" output="screen"/>
     <node name="MqttToRosNode" pkg="panda_mqtt_connector" type="MqttToRosNode" respawn="false" output="screen"/>
-    <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
     <node name="RobotStateProvider" pkg="panda_mqtt_connector" type="RobotStateProvider" respawn="false" output="screen"/>
+    <node name="safety_zone_spawner" pkg="panda_mqtt_connector" type="safety_zone_spawner" respawn="false" output="screen"/>
+    <node name="MqttRosConnectionTestNode" pkg="panda_mqtt_connector" type="MqttRosConnectionTestNode" respawn="false" output="screen"/>
 </launch>
diff --git a/mainpage.dox b/mainpage.dox
new file mode 100644
index 0000000..072ec45
--- /dev/null
+++ b/mainpage.dox
@@ -0,0 +1,28 @@
+/**
+\mainpage
+
+\htmlinclude manifest.html
+
+## Provided Nodes
+
+### safety_zone_spawner
+
+* **Author:** Johannes Mey*
+
+This node spawns cubic safety zones in gazebo.
+It expects the ROS parameter `/safety_zone_spawner/zone_size` and `/safety_zone_spawner/zones` to be set.
+
+- `/safety_zone_spawner/zone_size: double`
+
+  the edge size of a safety zone cube to be displayed in *m*.
+- `/safety_zone_spawner/zones: std::vector<std::string>`
+
+  a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
+  and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
+  are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
+  `x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
+  they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
+  given here and the `zone_size`.
+
+
+*/
diff --git a/package.xml b/package.xml
index 7aa73dd..d9b8161 100644
--- a/package.xml
+++ b/package.xml
@@ -42,6 +42,7 @@
   <depend>moveit_visual_tools</depend>
   <depend>controller_interface</depend>
   <depend>hardware_interface</depend>
+  <depend>simulation_util</depend>
 
   <exec_depend>pluginlib</exec_depend>
 
diff --git a/src/safety_zone_spawner.cpp b/src/safety_zone_spawner.cpp
new file mode 100644
index 0000000..aa20982
--- /dev/null
+++ b/src/safety_zone_spawner.cpp
@@ -0,0 +1,87 @@
+/*! \file safety_zone_spawner.cpp
+    \brief ROS node main file that spawns safety zones in gazebo.
+
+    \author Johannes Mey
+    \date 28.06.20
+*/
+
+#include <ros/ros.h>
+#include <shape_msgs/SolidPrimitive.h>
+#include <geometry_msgs/Pose.h>
+#include <GazeboZoneSpawner.h>
+
+/**
+ * The main method of the ROS node
+ *
+ * It does not requite command-line parameters, but expects the ROS parameter `/safety_zone_spawner/zone_size` and
+ * `/safety_zone_spawner/zones` to be set.
+ *
+ *  - `/safety_zone_spawner/zone_size: double`
+ *
+ *    the edge size of a safety zone cube to be displayed in *m*.
+ *  - `/safety_zone_spawner/zones: std::vector<std::string>`
+ *
+ *    a list of safety zone cube integer coordinates. The coordinates are encoded in a string and separated by spaces
+ *    and have an `x`, a `y`, and and optionally a `z` coordinate. Examples are `"3 0"` or `"-5 2 4"`. The coordinates
+ *    are interpreted as follows: the cube at position "0 0 0" is centered at the origin of the coordinate system in
+ *    `x` and `y` direction, but is located *on top of the x-y-plane*. All other node coordinates are defined such that
+ *    they completely fill the space. This means that the actual coordinates (in *m*) can be computed using the ones
+ *    given here and the `zone_size`.
+ */
+int main(int argc, char **argv) {
+
+  // setup this ros-node
+  ros::init(argc, argv, "safety_zone_spawner");
+  ros::NodeHandle node_handle("panda_mqtt_connector");
+
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
+
+  // wait for gazebo
+  // TODO this should actually wait until gazebo has started
+  ros::Duration(2.0).sleep();
+
+
+  std::vector<std::string> keys;
+  node_handle.getParamNames(keys);
+  for (auto key : keys) {
+    ROS_ERROR_STREAM(key);
+  }
+
+  // get size
+  double size = 0.5;
+  if (!node_handle.getParam("zone_size", size)) {
+    ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/size from param server, defaulting to " << size);
+  }
+
+  shape_msgs::SolidPrimitive box;
+  box.type = shape_msgs::SolidPrimitive::BOX;
+  box.dimensions = {size, size, size};
+
+  // get list of zone voxels
+  std::vector<std::string> voxels;
+  if (!node_handle.getParam("zones", voxels)) {
+    ROS_WARN_STREAM("Could not get string value for " << node_handle.getNamespace() << "/zones from param server. Not adding any zones.");
+  }
+
+  for (const std::string &voxel: voxels) {
+    geometry_msgs::Pose pose;
+
+    std::stringstream stream(voxel);
+    int x, y, z = 0;
+    stream >> x >> y;
+    if (stream) {
+      stream >> z; // the z value is 0 by default
+    }
+
+    pose.position.x = x * size;
+    pose.position.y = y * size;
+    pose.position.z = z * size + size / 2;
+
+    // the orientation is always the default
+    pose.orientation.w = 1;
+
+    GazeboZoneSpawner::spawnCollisionBox(box, pose);
+  }
+
+}
\ No newline at end of file
-- 
GitLab