cleanups for demo for tutorial

.rosinstall

@@ -1,5 +1,5 @@
-# This file is intended for users who want to build MoveIt from source.
-# Used with wstool, users can download source of all packages of MoveIt!.
+# This file is intended for users who want to build MTC and its demos from source.
+# Using wstool, users can download relevant MTC-related source packages.
 - git:
     local-name: moveit_task_constructor
     uri: https://github.com/ros-planning/moveit_task_constructor.git
@@ -12,7 +12,3 @@
     local-name: mtc_pour
     uri: https://github.com/TAMS-Group/mtc_pour.git
     version: master
-- git:
-    local-name: rosparam-shortcuts
-    uri: https://github.com/PickNikRobotics/rosparam_shortcuts.git
-    version: melodic-devel

demo/CMakeLists.txt

@@ -7,7 +7,6 @@ find_package(catkin REQUIRED COMPONENTS
 	roscpp
 	moveit_core
 	moveit_task_constructor_core
-	moveit_visual_tools
 	moveit_ros_planning_interface
 	rosparam_shortcuts
 )
@@ -20,24 +19,19 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME}
-  src/pick_place_task.cpp
-)
-
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-add_executable(${PROJECT_NAME}_moveit_task_constructor_demo src/moveit_task_constructor_demo.cpp)
-
-set_target_properties(${PROJECT_NAME}_moveit_task_constructor_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo PREFIX "")
-
-add_dependencies(${PROJECT_NAME}_moveit_task_constructor_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_library(${PROJECT_NAME}_lib src/pick_place_task.cpp)
+set_target_properties(${PROJECT_NAME}_lib PROPERTIES OUTPUT_NAME moveit_task_constructor_demo_pick_place)
+add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-target_link_libraries(${PROJECT_NAME}_moveit_task_constructor_demo
-  ${PROJECT_NAME}
+add_executable(${PROJECT_NAME}_demo src/moveit_task_constructor_demo.cpp)
+target_link_libraries(${PROJECT_NAME}_demo
+  ${PROJECT_NAME}_lib
   ${catkin_LIBRARIES}
 )
+set_target_properties(${PROJECT_NAME}_demo PROPERTIES OUTPUT_NAME moveit_task_constructor_demo)
+add_dependencies(${PROJECT_NAME}_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo
+install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_demo
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -46,10 +40,8 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_moveit_task_constructor_demo
 install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"
-  PATTERN ".svn" EXCLUDE
 )
 
-install(FILES
-  launch/demo.launch
+install(DIRECTORY launch config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

demo/config/mtc.rviz

@@ -5,35 +5,37 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Motion Planning Tasks1
-      Splitter Ratio: 0.7425600290298462
-    Tree Height: 270
+        - /Motion Planning Tasks1/Markers1
+      Splitter Ratio: 0.5393258333206177
+    Tree Height: 533
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz_visual_tools/RvizVisualToolsGui
-    Name: RvizVisualToolsGui
   - Class: moveit_task_constructor/Motion Planning Tasks
     Global Settings:
       Task View Settings:
         Task Expansion: All Expanded
     Name: Motion Planning Tasks
     Tasks View:
       property_splitter:
-        - 87
-        - 87
+        - 541
+        - 0
       solution_sorting:
         column: 0
         order: 1
       solutions_splitter:
-        - 302
-        - 70
-      solutions_view_columns: ~
+        - 328
+        - 76
+      solutions_view_columns:
+        - 38
+        - 0
+        - 0
       tasks_view_columns:
-        - 153
+        - 250
         - 38
         - 38
 Preferences:
@@ -68,6 +70,7 @@ Visualization Manager:
       Markers:
         All at once?: false
         Value: true
+        approach_object: true
       Name: Motion Planning Tasks
       Robot:
         Fixed Robot Color: 150; 50; 150
@@ -153,7 +156,7 @@ Visualization Manager:
       State Display Time: 0.05 s
       Task Solution Topic: /moveit_task_constructor_demo/pick_place_task/solution
       Tasks:
-        {}
+        pick_place_task: 10
       Trail Step Size: 1
       Value: true
   Enabled: true
@@ -168,35 +171,34 @@ Visualization Manager:
       Hide Inactive Objects: true
     - Class: rviz/MoveCamera
     - Class: rviz/Select
-    - Class: rviz_visual_tools/KeyTool
   Value: true
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 2.827594041824341
+      Distance: 1.9269260168075562
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.11356700211763382
-        Y: 0.10592000186443329
+        X: 0.8292451500892639
+        Y: -0.2452867180109024
         Z: 2.2351800055275817e-7
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.47020357847213745
+      Pitch: 0.3197975158691406
       Target Frame: panda_link0
       Value: XYOrbit (rviz)
-      Yaw: 6.06496000289917
+      Yaw: 6.239960670471191
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 741
+  Height: 768
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -205,11 +207,9 @@ Window Geometry:
     collapsed: false
   Motion Planning Tasks - Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001780000028bfc020000000ffb000000100044006900730070006c006100790073010000003d0000019f000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000004100fffffffb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b007301000001e2000000e6000000e600ffffff000003d80000028b00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  RvizVisualToolsGui:
-    collapsed: false
+  QMainWindow State: 000000ff00000000fd000000020000000000000166000002a6fc020000000efb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff00000000000000000000000100000198000002a6fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d00000254000000e500fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002970000004c0000004100ffffff0000028f000002a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
-  Width: 1366
-  X: 544
-  Y: 315
+  Width: 1433
+  X: 472
+  Y: 25

demo/launch/demo.launch

@@ -18,7 +18,7 @@
   <!-- Given the published joint states, publish tf for the robot links -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
-  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <!-- Run the main MoveIt executable (the move_group node) -->
   <include file="$(find panda_moveit_config)/launch/move_group.launch">
     <arg name="allow_trajectory_execution" value="true"/>
     <arg name="fake_execution" value="true"/>

demo/package.xml

@@ -8,15 +8,12 @@
   <author email="henningkayser@picknik.ai">Henning Kayser</author>
   <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>BSD</license>
 
   <buildtool_depend>catkin</buildtool_depend>
   <depend>moveit_task_constructor_core</depend>
-  <depend>moveit_visual_tools</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_core</depend>
   <depend>rosparam_shortcuts</depend>
+  <exec_depend version_gte="0.7.4">panda_moveit_config</exec_depend>
 </package>

demo/src/moveit_task_constructor_demo.cpp

@@ -57,12 +57,12 @@ moveit_msgs::CollisionObject createTable() {
 	std::string table_name, table_reference_frame;
 	std::vector<double> table_dimensions;
 	geometry_msgs::Pose pose;
-	std::size_t error = 0;
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
-	rosparam_shortcuts::shutdownIfError(LOGNAME, error);
+	std::size_t errors = 0;
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose);
+	rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
 
 	moveit_msgs::CollisionObject object;
 	object.id = table_name;

demo/src/pick_place_task.cpp

@@ -52,34 +52,34 @@ void PickPlaceTask::loadParameters() {
 	ros::NodeHandle pnh("~");
 
 	// Planning group properties
-	size_t error = 0;
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_);
+	size_t errors = 0;
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_);
 
 	// Predefined pose targets
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_);
 
 	// Target object
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_);
 	support_surfaces_ = { surface_link_ };
 
 	// Pick/Place metrics
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_);
-	error += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_);
-	rosparam_shortcuts::shutdownIfError(LOGNAME, error);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_);
+	errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_);
+	rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
 }
 
 void PickPlaceTask::init() {