Adds .panda_simulation folder creation functionality

The robot_control_node.cpp file now first tries to add the .panda_simulation
folder before throwing an error.

CMakeLists.txt

@@ -40,7 +40,7 @@ message(${JSONCPP_LIBRARIES})
 ## The catkin_package macro generates cmake config files for your package
 ## Declare things to be passed to dependent projects
 catkin_package(
-  CATKIN_DEPENDS 
+  CATKIN_DEPENDS
   moveit_core
   moveit_visual_tools
   moveit_ros_planning_interface

launch/simulation.launch

@@ -37,7 +37,7 @@
         <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" >
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
         <arg name="load_gripper" value="$(arg load_gripper)" />
     </include>
     <group if="$(arg gui)">
@@ -45,8 +45,8 @@
     </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" />
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen"/>
 
 </launch>
\ No newline at end of file

src/robot_control_node.cpp

@@ -80,8 +80,24 @@ int main(int argc, char **argv)
 
     if (!fs::exists(app_directory) && !fs::is_directory(app_directory))
     {
-      ROS_ERROR_STREAM(app_directory << " does not exist");
-      return -1;
+      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;