diff --git a/CMakeLists.txt b/CMakeLists.txt index bc6fad654972261cc60cac3b1ae5c9467d077dc0..4357e7153ea0b4922b714261c2103525f9f6a814 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/launch/simulation.launch b/launch/simulation.launch index c7f3b0922c911e45eccb0f008a5eb45885131ff8..b3471f2a727774c667f00f318667ca4808d38615 100644 --- a/launch/simulation.launch +++ b/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 diff --git a/src/robot_control_node.cpp b/src/robot_control_node.cpp index e1f857b70a6cfc3a0015986e9f17673337805f8f..beb175b32de85e11c68c7227be65ff7dcdcb068c 100644 --- a/src/robot_control_node.cpp +++ b/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;