include gazebo zone spawner in safety aware planner

CMakeLists.txt

@@ -77,6 +77,6 @@ add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp)
 target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES} ${SDF_LIBRARIES} GazeboZoneSpawner)
 target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
 target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
-target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES} SafetyEnvironmentCreator)
+target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES} SafetyEnvironmentCreator GazeboZoneSpawner)
 # target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
 

launch/simulation_safety.launch

+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+    <!-- launch robot control node for moveit motion planning -->
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node name="SampleConstraintPlanner" pkg="panda_simulation" type="SafetyAwarePlanner" respawn="false" output="screen"/>
+
+</launch>

src/SafetyAwarePlanner.cpp

@@ -4,6 +4,7 @@
 #include <moveit_visual_tools/moveit_visual_tools.h>
 
 #include "SafetyZones/SafetyEnvironmentCreator.cpp"
+#include "SafetyZones/GazeboZoneSpawner.h"
 
 int main(int argc, char** argv)
 {
@@ -61,6 +62,9 @@ int main(int argc, char** argv)
     SafetyEnvironmentCreator sec(safety_box, safety_box_pose);
     std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
 
+    // show safety box in gazebo
+    GazeboZoneSpawner::spawnCollisionBox(safety_box, safety_box_pose);
+
     //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
     // visual_tools.setAlpha(0.0);
     // Now, let's add the collision object into the world