diff --git a/CMakeLists.txt b/CMakeLists.txt
index 266446ffb111b659caccbc9a3497033ada603f94..95a10d81df1c6b7895d09024a1bbf57280c52c96 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -60,23 +60,11 @@ include_directories(${catkin_INCLUDE_DIRS} ${SDF_INCLUDE_DIRS})
# add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
-add_library(GazeboZoneSpawner src/SafetyZones/GazeboZoneSpawner.cpp src/SafetyZones/GazeboZoneSpawner.h)
-add_library(SafetyEnvironmentCreator src/SafetyZones/SafetyEnvironmentCreator.cpp src/SafetyZones/SafetyEnvironmentCreator.h)
# Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
-target_link_libraries(GazeboZoneSpawner ${catkin_LIBRARIES} ${SDF_LIBRARIES})
-target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES})
-add_executable(robot_control_node src/robot_control_node.cpp)
-add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
-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 GazeboZoneSpawner)
-# target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
diff --git a/README.md b/README.md
index de43529039e3e888ad1fe31e675f981731e3d0f4..389f49b3cb79679982d777abf8a8eb8b141bed80 100644
--- a/README.md
+++ b/README.md
@@ -1,80 +1,3 @@
# panda_simulation
-
-This package was written for ROS melodic running under Ubuntu 18.04. Run the following commands to make sure that all additional packages are installed:
-
-```
-mkdir -p catkin_ws/src
-cd catkin_ws/src
-git clone https://github.com/erdalpekel/panda_simulation.git
-git clone https://github.com/erdalpekel/panda_moveit_config.git
-git clone --branch simulation https://github.com/erdalpekel/franka_ros.git
-cd ..
-sudo apt-get install libboost-filesystem-dev
-rosdep install --from-paths src --ignore-src -y --skip-keys libfranka
-cd ..
-```
-It is also important that you build the *libfranka* library from source and pass its directory to *catkin_make* when building this ROS package as described in [this tutorial](https://frankaemika.github.io/docs/installation.html#building-from-source).
-
-Currently it includes a controller parameter config file and a launch file to launch the [Gazebo](http://gazebosim.org) simulation environment and the Panda robot from FRANKA EMIKA in it with the necessary controllers.
-
-Build the catkin workspace and run the simulation:
-```
-catkin_make -j4 -DCMAKE_BUILD_TYPE=Release -DFranka_DIR:PATH=/path/to/libfranka/build
-source devel/setup.bash
-roslaunch panda_simulation simulation.launch
-```
-
-Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:
-
-```
-export LC_NUMERIC="en_US.UTF-8"
-```
-==== BASE ====
-Otherwise, the robot will appear in rviz in a collapsed state.
-
-
-You can see the full explanation in my [blog post](https://erdalpekel.de/?p=55).
-
-## Changelog:
-
- #### Safetyzones
-
- 
-
- Integrates safetyzones via the SafetyAwarePlanner.
- start-command:
- ...
- roslaunch panda_simulation simulation.launch
- ...
-
- To see through the boxes one have to set the alpha of the planning an environment (see picture).
-
- #### _MoveIt!_ constraint-aware planning
-
- An example of one can add constraints to the planning and simulate it. start-command:
- start-command:
- ...
- roslaunch panda_simulation simulation_constraint.launch
- ...
-
- This repository was extended with a ROS node that communicates with the _MoveIt!_ Planning Scene API. It makes sure that the motion planning pipeline avoids collision objects in the environment specified by the user in a separate directory (`~/.panda_simulation`) as _json_ files.
-
- [Publishing a box at Panda's hand in _Gazebo_](https://erdalpekel.de/?p=123)
-
- This repository was extended with a node that publishes a simple box object in the _Gazebo_ simulation at the hand of the robot. The position of this box will get updated as soon as the robot moves.
-
- [Visual Studio Code Remote Docker](https://erdalpekel.de/?p=123)
-
- I have added configuration files and additional setup scripts for developing and using this ROS package within a *Docker* container. Currently user interfaces for Gazebo and RViz are not supported.
-
- [Position based trajectory execution](https://erdalpekel.de/?p=285)
-
- The joint specifications in Gazebo were changed from an effort interface to position based interface. Furthermore, the PID controller was substituted with the simple gazebo internal position based control mechanism for a more stable movement profile of the robot. A custom joint position based controller was implemented in order to set the initial joint states of the robot to a valid configuration.
-
- [Automatic robot state initialization](https://erdalpekel.de/?p=314)
-
- A separate ROS node was implemented that starts a custom joint position controller and initializes the robot with a specific configuration. It switches back to the default controllers after the robot reaches the desired state.
-
-
diff --git a/assets/box.png b/assets/box.png
deleted file mode 100644
index 391c0c2e3b35358d689f769308d4b6059466aae4..0000000000000000000000000000000000000000
Binary files a/assets/box.png and /dev/null differ
diff --git a/assets/robot-state-initializer.gif b/assets/robot-state-initializer.gif
deleted file mode 100644
index 2a570fd015bfca8a83b30a569abe35cfa1fb7b89..0000000000000000000000000000000000000000
Binary files a/assets/robot-state-initializer.gif and /dev/null differ
diff --git a/launch/box.launch b/launch/box.launch
deleted file mode 100644
index 8ead98ea44fa54d33a1c0a5cc96e226f4921343d..0000000000000000000000000000000000000000
--- a/launch/box.launch
+++ /dev/null
@@ -1,9 +0,0 @@
-<launch>
- <param name="box_description" command="$(find xacro)/xacro $(find panda_simulation)/models/box.xacro"/>
- <node name="box_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" args="robot_description:=box_description" />
- <node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-param box_description -urdf -model box"/>
-
-
- <!-- launch node -->
- <node pkg="panda_simulation" type="box_publisher_node" name="box_publisher_node" />
-</launch>
\ No newline at end of file
diff --git a/launch/move_group_interface_tutorial.launch b/launch/move_group_interface_tutorial.launch
deleted file mode 100644
index 1fddc14f2140fb97e3673115025337328d043438..0000000000000000000000000000000000000000
--- a/launch/move_group_interface_tutorial.launch
+++ /dev/null
@@ -1,66 +0,0 @@
-<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 pkg="panda_simulation" type="move_group_interface_tutorial" name="move_group_interface_tutorial" respawn="false" output="screen">
-
- </node>
-
-</launch>
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 0639685f9548bfa2b2b848230f5f1cbecc984675..f1c665e7743790e4b8dd1c277a75434212c73b72 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -44,9 +44,6 @@
<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" />
@@ -59,7 +56,4 @@
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
- <node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/>
-
-
</launch>
diff --git a/launch/simulation_constraint.launch b/launch/simulation_constraint.launch
deleted file mode 100644
index 104280310e0a4d95470caa745bec8dc0c574f966..0000000000000000000000000000000000000000
--- a/launch/simulation_constraint.launch
+++ /dev/null
@@ -1,62 +0,0 @@
-<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="SampleConstraintPlanner" respawn="false" output="screen"/>
-
-</launch>
diff --git a/launch/simulation_safety.launch b/launch/simulation_safety.launch
deleted file mode 100644
index 3ada2b9329e64973399cb77f1b5d90cc9e19da17..0000000000000000000000000000000000000000
--- a/launch/simulation_safety.launch
+++ /dev/null
@@ -1,62 +0,0 @@
-<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>
diff --git a/models/box.sdf b/models/box.sdf
deleted file mode 100644
index ce165a33b0bf00e6dd43ada6551c4945364b5eec..0000000000000000000000000000000000000000
--- a/models/box.sdf
+++ /dev/null
@@ -1,22 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
- <model name="box">
- <static>true</static>
- <link name="link">
- <visual name="visual">
- <material>
- <script>
- <uri>/usr/share/gazebo-9/media/materials/scripts/gazebo.material</uri>
- <name>Gazebo/DarkMagentaTransparent</name>
- </script>
- </material>
- <pose>2 3 0.5 0 0 0</pose>
- <geometry>
- <box>
- <size>1 1 1</size>
- </box>
- </geometry>
- </visual>
- </link>
- </model>
-</sdf>
\ No newline at end of file
diff --git a/models/box.xacro b/models/box.xacro
deleted file mode 100644
index 4cfb076b1bce798a603542f04de35a14eb42e018..0000000000000000000000000000000000000000
--- a/models/box.xacro
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
- <link name="object_base_link">
- </link>
-
- <joint name="object_base_joint" type="fixed">
- <parent link="object_base_link"/>
- <child link="object_link"/>
- <axis xyz="0 0 1" />
- <origin xyz="0 0 0" rpy="0 0 0"/>
- </joint>
-
- <link name="object_link">
- <inertial>
- <origin xyz="0 0 0" />
- <mass value="1.0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
- </inertial>
- <visual>
- <origin xyz="0 0 0"/>
- <geometry>
- <box size="0.04 0.04 0.08" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0"/>
- <geometry>
- <box size="0.04 0.04 0.08" />
- </geometry>
- </collision>
- </link>
-
- <gazebo reference="object_base_link">
- <gravity>0</gravity>
- </gazebo>
-
- <gazebo reference="object_link">
- <material>Gazebo/Blue</material>
- </gazebo>
-</robot>
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 3a9bafe7639f2416a6ffb1287aa1428a4d3518d1..70e78ae73207a2db3e74719bef533d47e0352e7a 100644
--- a/package.xml
+++ b/package.xml
@@ -31,7 +31,6 @@
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>xacro</depend>
- <depend>libjsoncpp-dev</depend>
<depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM -->
@@ -42,10 +41,6 @@
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
-
- <build_depend>sdformat</build_depend>
- <exec_depend>sdformat</exec_depend>
-
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>pluginlib</exec_depend>
diff --git a/src/SafetyAwarePlanner.cpp b/src/SafetyAwarePlanner.cpp
deleted file mode 100644
index 61cd8b74a25c7d38be4d0d3a49c2f72e31713a0c..0000000000000000000000000000000000000000
--- a/src/SafetyAwarePlanner.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-#include "SafetyZones/SafetyEnvironmentCreator.cpp"
-#include "SafetyZones/GazeboZoneSpawner.h"
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "CONSTRAINT PLANNER");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- static const std::string PLANNING_GROUP = "panda_arm";
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization Setup
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- //visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
- visual_tools.setAlpha(0.5);
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
- ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
- ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- shape_msgs::SolidPrimitive safety_box;
- geometry_msgs::Pose safety_box_pose;
-
- safety_box_pose.orientation.w = 1.0;
- safety_box_pose.orientation.x = 0.0;
- safety_box_pose.orientation.y = 0.0;
- safety_box_pose.orientation.z = 0.0;
- safety_box_pose.position.x = 0.0;
- safety_box_pose.position.y = 0.0;
- safety_box_pose.position.z = 0.0;
-
- safety_box.type = safety_box.BOX;
- safety_box.dimensions.resize(3);
-
- safety_box.dimensions[0] = 2.0;
- safety_box.dimensions[1] = 2.0;
- safety_box.dimensions[2] = 2.0;
-
- // Safety Box
- SafetyEnvironmentCreator sec(safety_box, safety_box_pose);
- std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);
-
- // show safety box in gazebo
- safety_box_pose.position.z += .5 * safety_box.BOX_Z;
- 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
- ROS_INFO_NAMED("tutorial", "Add an object into the world");
-
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- //moveit_msgs::MotionPlanRequest mr;
- //mr.max_velocity_scaling_factor = 0.5;
- //move_group.constructMotionPlanRequest(mr);
-
- //move_group.setMaxVelocityScalingFactor(0.1);
- //move_group.setWorkspace(-0.5, -0.5, -0.5, 0, 0, 0);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
-
- visual_tools.prompt("Press 'next' to move the simulated robot.");
- visual_tools.trigger();
-
- // Move the simulated robot in gazebo
- move_group.move();
-
- ros::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/src/SafetyZones/GazeboZoneSpawner.cpp b/src/SafetyZones/GazeboZoneSpawner.cpp
deleted file mode 100644
index ea268c9bd37d589efd99e92e04a814ccb5bbc187..0000000000000000000000000000000000000000
--- a/src/SafetyZones/GazeboZoneSpawner.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-//
-// Created by Johannes Mey on 31.03.20.
-//
-
-
-#include "GazeboZoneSpawner.h"
-
-#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
-#include <gazebo_msgs/SpawnModel.h> // for spawning stuff in gazebo
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <tf/tf.h>
-
-void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
- if (shape.type != shape.BOX) {
- ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
- return;
- }
-
- sdf::SDFPtr sdf(new sdf::SDF());
- sdf::init(sdf);
-
- auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
-
- assert(sdf::readFile(sdfFileName, sdf));
-
- auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
- auto poseElement = visualElement->GetElement("pose");
- auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
-
- tf::Quaternion rot;
- rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
- double roll, pitch, yaw;
- tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
-
- std::ostringstream poseStream;
- poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " " << roll << " " << pitch << " " << yaw;
- poseElement->Set(poseStream.str());
-
- ROS_INFO_NAMED("GazeboZoneSpawner", "Set pose in SDF file to '%s'.", poseElement->GetValue()->GetAsString().c_str());
-
- std::ostringstream sizeStream;
- sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
- sizeElement->Set(sizeStream.str());
-
- ROS_INFO_NAMED("GazeboZoneSpawner", "Set size in SDF file to '%s'.", sizeElement->GetValue()->GetAsString().c_str());
-
- ros::NodeHandle n;
- ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
- gazebo_msgs::SpawnModel spawnService;
- spawnService.request.robot_namespace = "box space";
- // spawnModelService.request.initial_pose = pose; // not required here
- spawnService.request.model_name = std::string("box") + poseStream.str() + sizeStream.str();
- spawnService.request.model_xml = sdf->ToString().c_str();
- // spawnModelService.request.reference_frame = ; // if left empty, world is used
- if (client.call(spawnService)) {
- if ( spawnService.response.success) {
- ROS_INFO_NAMED("GazeboZoneSpawner", "Successfully spawned Box 's'. %s", spawnService.request.model_name.c_str(), spawnService.response.status_message.c_str());
- } else {
- ROS_INFO_NAMED("GazeboZoneSpawner", "Failed to spawn Box '%s'. %s", spawnService.request.model_name.c_str(), spawnService.response.status_message.c_str());
- }
- } else {
- ROS_ERROR_NAMED("GazeboZoneSpawner", "Failed to call service '/gazebo/spawn_sdf_model'");
- }
-}
diff --git a/src/SafetyZones/GazeboZoneSpawner.h b/src/SafetyZones/GazeboZoneSpawner.h
deleted file mode 100644
index b0c97a923e83be2e32291869c7d62bd30d2e1396..0000000000000000000000000000000000000000
--- a/src/SafetyZones/GazeboZoneSpawner.h
+++ /dev/null
@@ -1,20 +0,0 @@
-//
-// Created by Johannes Mey on 31.03.20.
-//
-
-#ifndef PANDA_SIMULATION_GAZEBOZONESPAWNER_H
-#define PANDA_SIMULATION_GAZEBOZONESPAWNER_H
-
-
-#include <shape_msgs/SolidPrimitive.h>
-#include <geometry_msgs/Pose.h>
-
-class GazeboZoneSpawner {
-
-public:
- static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
-
-};
-
-
-#endif //PANDA_SIMULATION_GAZEBOZONESPAWNER_H
diff --git a/src/SafetyZones/SafetyEnvironmentCreator.cpp b/src/SafetyZones/SafetyEnvironmentCreator.cpp
deleted file mode 100644
index 76835262f9043a0ecb5f52dfc28e40003fd253cb..0000000000000000000000000000000000000000
--- a/src/SafetyZones/SafetyEnvironmentCreator.cpp
+++ /dev/null
@@ -1,250 +0,0 @@
-//
-// Created by Sebastian Ebert
-//
-
-#include "SafetyEnvironmentCreator.h"
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-
-SafetyEnvironmentCreator::SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
- if(shape.type != shape.BOX){
- ROS_ERROR("Safety-zone could not be created due to wrong shape-type");
- safety_zone_depth = 0.0;
- safety_zone_width = 0.0;
- safety_zone_height = 0.0;
- zone_orientation_w = 1.0;
- zone_orientation_x = 1.0;
- zone_orientation_y = 1.0;
- zone_orientation_z = 1.0;
- }else {
- safety_zone_depth = shape.dimensions[0];
- safety_zone_width = shape.dimensions[1];
- safety_zone_height = shape.dimensions[2];
- zone_orientation_w = pose.orientation.w;
- zone_orientation_x = pose.orientation.x;
- zone_orientation_y = pose.orientation.y;
- zone_orientation_z = pose.orientation.z;
- zone_position_x = pose.position.x;
- zone_position_y = pose.position.y;
- zone_position_z = pose.position.z;
- }
-}
-
-double SafetyEnvironmentCreator::getZoneWidth() const {
- return safety_zone_width;
-}
-
-double SafetyEnvironmentCreator::getZoneHeight() const {
- return safety_zone_height;
-}
-
-double SafetyEnvironmentCreator::getZoneDepth() const {
- return safety_zone_depth;
-}
-
-double SafetyEnvironmentCreator::getZoneOrientationW() const {
- return zone_orientation_w;
-}
-
-double SafetyEnvironmentCreator::getZoneOrientationX() const {
- return zone_orientation_x;
-}
-
-double SafetyEnvironmentCreator::getZoneOrientationY() const {
- return zone_orientation_y;
-}
-
-double SafetyEnvironmentCreator::getZoneOrientationZ() const {
- return zone_orientation_z;
-}
-
-std::vector<moveit_msgs::CollisionObject>
-SafetyEnvironmentCreator::createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group) {
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
-
- // OBJECT 1 ( left box / -y )
- // ^^^^^^^^
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
-
- primitive.dimensions[0] = getZoneDepth() + 0.2; // x
- primitive.dimensions[1] = 0.1; // y
- primitive.dimensions[2] = getZoneHeight(); // z
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = zone_position_x;
- box_pose.position.y = zone_position_y + (-1 * ((getZoneWidth() / 2) + 0.05));
- box_pose.position.z = zone_position_z + (primitive.dimensions[2] / 2);
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- collision_objects.push_back(collision_object);
-
- // OBJECT 2 right box / +y )
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_2;
- collision_object_2.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_2.id = "box2";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_2;
- primitive_2.type = primitive.BOX;
- primitive_2.dimensions.resize(3);
- primitive_2.dimensions[0] = getZoneDepth() + 0.2;
- primitive_2.dimensions[1] = 0.1;
- primitive_2.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_2;
- box_pose_2.orientation.w = 1.0;
- box_pose_2.position.x = zone_position_x;
- box_pose_2.position.y = zone_position_y + ((getZoneWidth() / 2) + 0.05);
- box_pose_2.position.z = zone_position_z + (primitive_2.dimensions[2] / 2);
-
- collision_object_2.primitives.push_back(primitive_2);
- collision_object_2.primitive_poses.push_back(box_pose_2);
- collision_object_2.operation = collision_object_2.ADD;
-
- collision_objects.push_back(collision_object_2);
-
- // OBJECT 3 (front box / +x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_3;
- collision_object_3.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_3.id = "box3";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_3;
- primitive_3.type = primitive.BOX;
- primitive_3.dimensions.resize(3);
- primitive_3.dimensions[0] = 0.1;
- primitive_3.dimensions[1] = getZoneWidth() + 0.2;
- primitive_3.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_3;
- box_pose_3.orientation.w = 1.0;
- box_pose_3.position.x = zone_position_x + ((getZoneDepth() / 2) + 0.05);
- box_pose_3.position.y = zone_position_y;
- box_pose_3.position.z = zone_position_z + (primitive_3.dimensions[2] / 2);
-
- collision_object_3.primitives.push_back(primitive_3);
- collision_object_3.primitive_poses.push_back(box_pose_3);
- collision_object_3.operation = collision_object_3.ADD;
-
- collision_objects.push_back(collision_object_3);
-
- // OBJECT 4 (back box / -x)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_4;
- collision_object_4.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_4.id = "box4";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_4;
- primitive_4.type = primitive.BOX;
- primitive_4.dimensions.resize(3);
- primitive_4.dimensions[0] = 0.1;
- primitive_4.dimensions[1] = getZoneWidth() + 0.2;
- primitive_4.dimensions[2] = getZoneHeight();
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_4;
- box_pose_4.orientation.w = 1.0;
- box_pose_4.position.x = zone_position_x + (-1 * ((getZoneDepth() / 2) + 0.05));
- box_pose_4.position.y = zone_position_y;
- box_pose_4.position.z = zone_position_z + (primitive_4.dimensions[2] / 2);
-
- collision_object_4.primitives.push_back(primitive_4);
- collision_object_4.primitive_poses.push_back(box_pose_4);
- collision_object_4.operation = collision_object_4.ADD;
-
- collision_objects.push_back(collision_object_4);
-
- // OBJECT 5 (floor / -z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_5;
- collision_object_5.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_5.id = "box5";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_5;
- primitive_5.type = primitive.BOX;
- primitive_5.dimensions.resize(3);
- primitive_5.dimensions[0] = getZoneDepth() + 0.2;
- primitive_5.dimensions[1] = getZoneWidth() + 0.2;
- primitive_5.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_5;
- box_pose_5.orientation.w = 1.0;
- box_pose_5.position.x = zone_position_x;
- box_pose_5.position.y = zone_position_y;
- box_pose_5.position.z = zone_position_z - 0.05;
-
- collision_object_5.primitives.push_back(primitive_5);
- collision_object_5.primitive_poses.push_back(box_pose_5);
- collision_object_5.operation = collision_object_5.ADD;
-
- collision_objects.push_back(collision_object_5);
-
- // OBJECT 6 (top / +z)
- // ^^^^^^^^
- moveit_msgs::CollisionObject collision_object_6;
- collision_object_6.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object_6.id = "box6";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive_6;
- primitive_6.type = primitive.BOX;
- primitive_6.dimensions.resize(3);
- primitive_6.dimensions[0] = getZoneDepth() + 0.2;
- primitive_6.dimensions[1] = getZoneWidth() + 0.2;
- primitive_6.dimensions[2] = 0.1;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose_6;
- box_pose_6.orientation.w = 1.0;
- box_pose_6.position.x = zone_position_x;
- box_pose_6.position.y = zone_position_y;
- box_pose_6.position.z = zone_position_z + getZoneHeight() + 0.05;
-
- collision_object_6.primitives.push_back(primitive_6);
- collision_object_6.primitive_poses.push_back(box_pose_6);
- collision_object_6.operation = collision_object_6.ADD;
- collision_objects.push_back(collision_object_6);
-
- /*for(std::size_t i=0; i<collision_objects.size(); ++i) {
- collision_objects[i].primitive_poses[0].orientation.w = getZoneOrientationW();
- collision_objects[i].primitive_poses[0].orientation.x = getZoneOrientationX();
- collision_objects[i].primitive_poses[0].orientation.y = getZoneOrientationY();
- collision_objects[i].primitive_poses[0].orientation.z = getZoneOrientationZ();
- }*/
-
- return collision_objects;
-}
diff --git a/src/SafetyZones/SafetyEnvironmentCreator.h b/src/SafetyZones/SafetyEnvironmentCreator.h
deleted file mode 100644
index 6fbeb83d06c5d7929b0f08d6c5d44b43be17115d..0000000000000000000000000000000000000000
--- a/src/SafetyZones/SafetyEnvironmentCreator.h
+++ /dev/null
@@ -1,53 +0,0 @@
-//
-// Created by Johannes Mey on 31.03.20.
-//
-
-#ifndef PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
-#define PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-class SafetyEnvironmentCreator {
-
-private:
- double safety_zone_width;
- double safety_zone_height;
- double safety_zone_depth;
- double zone_orientation_w;
- double zone_orientation_x;
- double zone_orientation_y;
- double zone_orientation_z;
- double zone_position_x;
- double zone_position_y;
- double zone_position_z;
-public:
- double getZoneWidth() const;
-
- double getZoneHeight() const;
-
- double getZoneDepth() const;
-
- double getZoneOrientationW() const;
-
- double getZoneOrientationX() const;
-
- double getZoneOrientationY() const;
-
- double getZoneOrientationZ() const;
-
- /**
- * ctor
- * @param shape describes the inner dimensions of the safety zone
- * @param pose describes position and orientation of the safety zone
- * note: current zone-threshold is 0.1
- */
- SafetyEnvironmentCreator(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
-
-
- std::vector<moveit_msgs::CollisionObject>
- createCentralizedSafetyEnvironment(moveit::planning_interface::MoveGroupInterface &move_group);
-};
-
-#endif //PANDA_SIMULATION_SAFETYENVIRONMENTCREATOR_H
diff --git a/src/SampleConstraintPlanner.cpp b/src/SampleConstraintPlanner.cpp
deleted file mode 100644
index 59e35a4988a7540a2134a4d2454cb1c8b0a3b23b..0000000000000000000000000000000000000000
--- a/src/SampleConstraintPlanner.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-//
-// Created by sebastian on 27.03.20.
-//
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "CONSTRAINT PLANNER");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- static const std::string PLANNING_GROUP = "panda_arm";
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization Setup
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
- visual_tools.loadRemoteControl();
-
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
- ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
- ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = 0.4;
- primitive.dimensions[1] = 0.1;
- primitive.dimensions[2] = 0.4;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = 0.4;
- box_pose.position.y = -0.2;
- box_pose.position.z = 1.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- collision_objects.push_back(collision_object);
-
- // Now, let's add the collision object into the world
- ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
-
- visual_tools.prompt("Press 'next' to move the simulated robot.");
- visual_tools.trigger();
-
- // Move the simulated robot in gazebo
- move_group.move();
-
- ros::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/src/box_publisher.cpp b/src/box_publisher.cpp
deleted file mode 100644
index 339f3f292f420a8e37c8124975e7fb5191948c3a..0000000000000000000000000000000000000000
--- a/src/box_publisher.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-#include <gazebo_msgs/SetModelState.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/robot_model_loader/robot_model_loader.h>
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-
-static const std::string PLANNING_GROUP_ARM = "panda_arm";
-static const double PANDA_ARM_TO_HAND_OFFSET = 0.12;
-static const double PANDA_HAND_TO_FINGER_OFFSET = 0.04;
-ros::Publisher gazebo_model_state_pub;
-robot_model::RobotModelPtr kinematic_model;
-robot_state::RobotStatePtr kinematic_state;
-
-void jointStatesCallback(const sensor_msgs::JointState &joint_states_current)
-{
- const robot_state::JointModelGroup *joint_model_group = kinematic_model->getJointModelGroup("panda_arm");
- const std::vector<std::string> &joint_names = joint_model_group->getJointModelNames();
-
- std::vector<double> joint_states;
- for (size_t i = 0; i < joint_states_current.position.size() - 2; ++i)
- {
- joint_states.push_back(joint_states_current.position[i + 2]);
- }
- kinematic_state->setJointGroupPositions(joint_model_group, joint_states);
-
- const Eigen::Affine3d &end_effector_state = kinematic_state->getGlobalLinkTransform("panda_link8");
-
- double end_effector_z_offset = PANDA_ARM_TO_HAND_OFFSET + PANDA_HAND_TO_FINGER_OFFSET;
- Eigen::Affine3d tmp_transform(Eigen::Translation3d(Eigen::Vector3d(0.0, 0.0, end_effector_z_offset)));
-
- Eigen::Affine3d newState = end_effector_state * tmp_transform;
-
- geometry_msgs::Pose pose;
- pose.position.x = newState.translation().x();
- pose.position.y = newState.translation().y();
- pose.position.z = newState.translation().z();
-
- Eigen::Quaterniond quat(newState.rotation());
- pose.orientation.w = quat.w();
- pose.orientation.x = quat.x();
- pose.orientation.y = quat.y();
- pose.orientation.z = quat.z();
-
- ROS_DEBUG_STREAM("translation" << std::endl << newState.translation());
- ROS_DEBUG_STREAM("rotation" << std::endl << newState.rotation());
-
- gazebo_msgs::ModelState model_state;
- // This string results from the spawn_urdf call in the box.launch file argument: -model box
- model_state.model_name = std::string("box");
- model_state.pose = pose;
- model_state.reference_frame = std::string("world");
-
- gazebo_model_state_pub.publish(model_state);
-}
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "box_publisher_node");
- ros::NodeHandle node_handle;
-
- ros::Subscriber joint_states_sub = node_handle.subscribe("/joint_states", 1, jointStatesCallback);
-
- robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
- kinematic_model = robot_model_loader.getModel();
- kinematic_state = robot_state::RobotStatePtr(new robot_state::RobotState(kinematic_model));
-
- gazebo_model_state_pub = node_handle.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1);
-
- ros::spin();
- return 0;
-}
\ No newline at end of file
diff --git a/src/move_group_interface_tutorial.cpp b/src/move_group_interface_tutorial.cpp
deleted file mode 100644
index c98d8b26922f6137e73531b50e8fb33aa954b811..0000000000000000000000000000000000000000
--- a/src/move_group_interface_tutorial.cpp
+++ /dev/null
@@ -1,406 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2013, SRI International
- * 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 SRI International 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.
- *********************************************************************/
-
-/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */
-
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-
-#include <moveit_msgs/DisplayRobotState.h>
-#include <moveit_msgs/DisplayTrajectory.h>
-
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/CollisionObject.h>
-
-#include <moveit_visual_tools/moveit_visual_tools.h>
-
-int main(int argc, char** argv)
-{
- ros::init(argc, argv, "move_group_interface_tutorial");
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- // BEGIN_TUTORIAL
- //
- // Setup
- // ^^^^^
- //
- // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
- // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
- // are used interchangably.
- static const std::string PLANNING_GROUP = "panda_arm";
-
- // The :planning_interface:`MoveGroupInterface` class can be easily
- // setup using just the name of the planning group you would like to control and plan for.
- moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
-
- // We will use the :planning_interface:`PlanningSceneInterface`
- // class to add and remove collision objects in our "virtual world" scene
- moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
- // Raw pointers are frequently used to refer to the planning group for improved performance.
- const robot_state::JointModelGroup* joint_model_group =
- move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
-
- // Visualization
- // ^^^^^^^^^^^^^
- //
- // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
- // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
- namespace rvt = rviz_visual_tools;
- moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
- visual_tools.deleteAllMarkers();
-
- // Remote control is an introspection tool that allows users to step through a high level script
- // via buttons and keyboard shortcuts in RViz
- visual_tools.loadRemoteControl();
-
- // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
- Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
- text_pose.translation().z() = 1.75;
- visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);
-
- // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
- visual_tools.trigger();
-
- // Getting Basic Information
- // ^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // We can print the name of the reference frame for this robot.
- ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());
-
- // We can also print the name of the end-effector link for this group.
- ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
-
- // We can get a list of all the groups in the robot:
- ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
- std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
- std::ostream_iterator<std::string>(std::cout, ", "));
-
- // Start the demo
- // ^^^^^^^^^^^^^^^^^^^^^^^^^
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
-
- // Planning to a Pose goal
- // ^^^^^^^^^^^^^^^^^^^^^^^
- // We can plan a motion for this group to a desired pose for the
- // end-effector.
- geometry_msgs::Pose target_pose1;
- target_pose1.orientation.w = 1.0;
- target_pose1.position.x = 0.28;
- target_pose1.position.y = -0.2;
- target_pose1.position.z = 0.5;
- move_group.setPoseTarget(target_pose1);
-
- // Now, we call the planner to compute the plan and visualize it.
- // Note that we are just planning, not asking move_group
- // to actually move the robot.
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-
- bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
- ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
-
- // Visualizing plans
- // ^^^^^^^^^^^^^^^^^
- // We can also visualize the plan as a line with markers in RViz.
- ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
- visual_tools.publishAxisLabeled(target_pose1, "pose1");
- visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Moving to a pose goal
- // ^^^^^^^^^^^^^^^^^^^^^
- //
- // Moving to a pose goal is similar to the step above
- // except we now use the move() function. Note that
- // the pose goal we had set earlier is still active
- // and so the robot will try to move to that goal. We will
- // not use that function in this tutorial since it is
- // a blocking function and requires a controller to be active
- // and report success on execution of a trajectory.
-
- /* Uncomment below line when working with a real robot */
- move_group.move();
-
- // Planning to a joint-space goal
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Let's set a joint space goal and move towards it. This will replace the
- // pose target we set above.
- //
- // To start, we'll create an pointer that references the current robot's state.
- // RobotState is the object that contains all the current position/velocity/acceleration data.
- moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
- //
- // Next get the current set of joint values for the group.
- std::vector<double> joint_group_positions;
- current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
-
- // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
- joint_group_positions[0] = -1.0; // radians
- move_group.setJointValueTarget(joint_group_positions);
-
- // We lower the allowed maximum velocity and acceleration to 5% of their maximum.
- // The default values are 10% (0.1).
- // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
- // or set explicit factors in your code if you need your robot to move faster.
- move_group.setMaxVelocityScalingFactor(0.05);
- move_group.setMaxAccelerationScalingFactor(0.05);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Planning with Path Constraints
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Path constraints can easily be specified for a link on the robot.
- // Let's specify a path constraint and a pose goal for our group.
- // First define the path constraint.
- moveit_msgs::OrientationConstraint ocm;
- ocm.link_name = "panda_link7";
- ocm.header.frame_id = "panda_link0";
- ocm.orientation.w = 1.0;
- ocm.absolute_x_axis_tolerance = 0.1;
- ocm.absolute_y_axis_tolerance = 0.1;
- ocm.absolute_z_axis_tolerance = 0.1;
- ocm.weight = 1.0;
-
- // Now, set it as the path constraint for the group.
- moveit_msgs::Constraints test_constraints;
- test_constraints.orientation_constraints.push_back(ocm);
- move_group.setPathConstraints(test_constraints);
-
- // We will reuse the old goal that we had and plan to it.
- // Note that this will only work if the current state already
- // satisfies the path constraints. So we need to set the start
- // state to a new pose.
- robot_state::RobotState start_state(*move_group.getCurrentState());
- geometry_msgs::Pose start_pose2;
- start_pose2.orientation.w = 1.0;
- start_pose2.position.x = 0.55;
- start_pose2.position.y = -0.05;
- start_pose2.position.z = 0.8;
- start_state.setFromIK(joint_model_group, start_pose2);
- move_group.setStartState(start_state);
-
- // Now we will plan to the earlier pose target from the new
- // start state that we have just created.
- move_group.setPoseTarget(target_pose1);
-
- // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
- // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
- move_group.setPlanningTime(10.0);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishAxisLabeled(start_pose2, "start");
- visual_tools.publishAxisLabeled(target_pose1, "goal");
- visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("next step");
-
- // When done with the path constraint be sure to clear it.
- move_group.clearPathConstraints();
-
- // Cartesian Paths
- // ^^^^^^^^^^^^^^^
- // You can plan a Cartesian path directly by specifying a list of waypoints
- // for the end-effector to go through. Note that we are starting
- // from the new start state above. The initial pose (start state) does not
- // need to be added to the waypoint list but adding it can help with visualizations
- std::vector<geometry_msgs::Pose> waypoints;
- waypoints.push_back(start_pose2);
-
- geometry_msgs::Pose target_pose3 = start_pose2;
-
- target_pose3.position.z -= 0.2;
- waypoints.push_back(target_pose3); // down
-
- target_pose3.position.y -= 0.2;
- waypoints.push_back(target_pose3); // right
-
- target_pose3.position.z += 0.2;
- target_pose3.position.y += 0.2;
- target_pose3.position.x -= 0.2;
- waypoints.push_back(target_pose3); // up and left
-
- // We want the Cartesian path to be interpolated at a resolution of 1 cm
- // which is why we will specify 0.01 as the max step in Cartesian
- // translation. We will specify the jump threshold as 0.0, effectively disabling it.
- // Warning - disabling the jump threshold while operating real hardware can cause
- // large unpredictable motions of redundant joints and could be a safety issue
- moveit_msgs::RobotTrajectory trajectory;
- const double jump_threshold = 0.0;
- const double eef_step = 0.01;
- double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
- for (std::size_t i = 0; i < waypoints.size(); ++i)
- visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
- visual_tools.trigger();
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
-
- // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian
- // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
- // the trajectory manually, as described [here](https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4).
- // Pull requests are welcome.
-
- // You can execute a trajectory by wrapping it in a plan like this.
- moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
- cartesian_plan.trajectory_ = trajectory;
- move_group.execute(cartesian_plan);
-
- // Adding/Removing Objects and Attaching/Detaching Objects
- // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
- //
- // Define a collision object ROS message.
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = move_group.getPlanningFrame();
-
- // The id of the object is used to identify it.
- collision_object.id = "box1";
-
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = 0.4;
- primitive.dimensions[1] = 0.1;
- primitive.dimensions[2] = 0.4;
-
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = 1.0;
- box_pose.position.x = 0.4;
- box_pose.position.y = -0.2;
- box_pose.position.z = 1.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- collision_objects.push_back(collision_object);
-
- // Now, let's add the collision object into the world
- ROS_INFO_NAMED("tutorial", "Add an object into the world");
- planning_scene_interface.addCollisionObjects(collision_objects);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- // Wait for MoveGroup to receive and process the collision object message
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
-
- // Now when we plan a trajectory it will avoid the obstacle
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose;
- another_pose.orientation.w = 1.0;
- another_pose.position.x = 0.4;
- another_pose.position.y = -0.4;
- another_pose.position.z = 0.9;
- move_group.setPoseTarget(another_pose);
-
- success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
-
- // Visualize the plan in RViz
- visual_tools.deleteAllMarkers();
- visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
- visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
- visual_tools.trigger();
- visual_tools.prompt("next step");
-
- // Now, let's attach the collision object to the robot.
- ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
- move_group.attachObject(collision_object.id);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
- "robot");
-
- // Now, let's detach the collision object from the robot.
- ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
- move_group.detachObject(collision_object.id);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
- "robot");
-
- // Now, let's remove the collision object from the world.
- ROS_INFO_NAMED("tutorial", "Remove the object from the world");
- std::vector<std::string> object_ids;
- object_ids.push_back(collision_object.id);
- planning_scene_interface.removeCollisionObjects(object_ids);
-
- // Show text in RViz of status
- visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
- visual_tools.trigger();
-
- /* Wait for MoveGroup to receive and process the attached collision object message */
- visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
-
- // END_TUTORIAL
-
- ros::shutdown();
- return 0;
-}
diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp
deleted file mode 100644
index 1c98ec7349271327c9273beb44aa5674d16280a6..0000000000000000000000000000000000000000
--- a/src/robot_control_node.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#include <jsoncpp/json/json.h>
-#include <moveit/move_group_interface/move_group_interface.h>
-#include <moveit/planning_scene/planning_scene.h>
-#include <moveit/planning_scene_interface/planning_scene_interface.h>
-#include <moveit_visual_tools/moveit_visual_tools.h>
-#include <ros/ros.h>
-#include <boost/filesystem.hpp>
-
-#include "SafetyZones/GazeboZoneSpawner.h"
-
-static const std::string PLANNING_GROUP_ARM = "panda_arm";
-static const std::string APP_DIRECTORY_NAME = ".panda_simulation";
-
-
-
-moveit_msgs::CollisionObject extractObstacleFromJson(Json::Value &root, std::string name)
-{
- moveit_msgs::CollisionObject collision_object;
- collision_object.header.frame_id = "world";
- collision_object.id = name;
-
- const Json::Value dimensions = root["dimensions"];
- ROS_INFO_STREAM("Extracted dimensions: " << dimensions);
- // Define a box to add to the world.
- shape_msgs::SolidPrimitive primitive;
- primitive.type = primitive.BOX;
- primitive.dimensions.resize(3);
- primitive.dimensions[0] = dimensions["x"].asDouble();
- primitive.dimensions[1] = dimensions["y"].asDouble();
- primitive.dimensions[2] = dimensions["z"].asDouble();
-
- const Json::Value position = root["position"];
- ROS_INFO_STREAM("Extracted position: " << position);
-
- const Json::Value orientation = root["orientation"];
- ROS_INFO_STREAM("Extracted orientation: " << orientation);
- // Define a pose for the box (specified relative to frame_id)
- geometry_msgs::Pose box_pose;
- box_pose.orientation.w = orientation["w"].asDouble();
- box_pose.orientation.x = orientation["x"].asDouble();
- box_pose.orientation.y = orientation["y"].asDouble();
- box_pose.orientation.z = orientation["z"].asDouble();
-
- // MoveIt! planning scene expects the center of the object as position.
- // We add half of its dimension to its position
- box_pose.position.x = position["x"].asDouble() + primitive.dimensions[0] / 2.0;
- box_pose.position.y = position["y"].asDouble() + primitive.dimensions[1] / 2.0;
- box_pose.position.z = position["z"].asDouble() + primitive.dimensions[2] / 2.0;
-
- collision_object.primitives.push_back(primitive);
- collision_object.primitive_poses.push_back(box_pose);
- collision_object.operation = collision_object.ADD;
-
- GazeboZoneSpawner::spawnCollisionBox(primitive, box_pose);
-
- return std::move(collision_object);
-}
-
-int main(int argc, char **argv)
-{
- namespace fs = boost::filesystem;
- ROS_INFO("RUNNING robot_control_node");
-
- ros::init(argc, argv, "robot_control_node");
-
- ros::NodeHandle node_handle;
- ros::AsyncSpinner spinner(1);
- spinner.start();
-
- moveit::planning_interface::MoveGroupInterface move_group_arm(PLANNING_GROUP_ARM);
-
- ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
- ros::WallDuration sleep_t(0.5);
- while (planning_scene_diff_publisher.getNumSubscribers() < 1)
- {
- sleep_t.sleep();
- }
- moveit_msgs::PlanningScene planning_scene;
-
- // read JSON files from ~/.panda_simulation
- fs::path home(getenv("HOME"));
- if (fs::is_directory(home))
- {
- fs::path app_directory(home);
- app_directory /= APP_DIRECTORY_NAME;
-
- if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
- {
- ROS_WARN_STREAM(app_directory << " does not exist");
-
- // Create .panda_simulation directory
- std::string path(getenv("HOME"));
- path += "/.panda_simulation";
- ROS_INFO("Creating %s collision objects directory.", path);
- try
- {
- boost::filesystem::create_directory(path);
- }
- catch (const std::exception&)
- {
- ROS_ERROR(
- "%s directory could not be created."
- "Please create this directory yourself "
- "if you want to specify collision objects.", path.c_str());
- return -1;
- }
- }
-
- std::vector<moveit_msgs::CollisionObject> collision_objects;
- ROS_INFO_STREAM(app_directory << " is a directory containing:");
- for (auto &entry : boost::make_iterator_range(fs::directory_iterator(app_directory), {}))
- {
- ROS_INFO_STREAM(entry);
-
- std::ifstream file_stream(entry.path().string(), std::ifstream::binary);
- if (file_stream)
- {
- Json::Value root;
- file_stream >> root;
-
- moveit_msgs::CollisionObject collision_object = extractObstacleFromJson(root, entry.path().stem().string());
- collision_objects.push_back(collision_object);
- }
- else
- {
- ROS_WARN_STREAM("could not open file " << entry.path());
- }
- }
-
- // Publish the collision objects to the scene
- for (const auto &collision_object : collision_objects)
- {
- collision_object.header.frame_id = move_group_arm.getPlanningFrame();
- planning_scene.world.collision_objects.push_back(collision_object);
- }
-
- ROS_INFO_STREAM("# collision objects " << planning_scene.world.collision_objects.size());
- planning_scene.is_diff = true;
- planning_scene_diff_publisher.publish(planning_scene);
-
- ROS_INFO("robot_control_node is ready");
- ros::waitForShutdown();
-
- return 0;
- }
-}
\ No newline at end of file