Update advanced exercises

- update CMakeLists/package.xml to latest templates - clean up minor whitespace inconsistencies - fix misc warnings (e.g. Eigen3, missing xacro: prefix) - move Perception to Exercise 5.1, to match wiki - Move STOMP to Exercise 5.2, to match wiki - add new Exercise 5.4: python/OpenCV - update instructions for Ex 5.0, 5.1, 5.4, 6.1
ros-industrial · Aug 30, 2018 · 23fd1ea · 23fd1ea
1 parent 3d47aa1
commit 23fd1ea
Show file tree

Hide file tree

Showing 87 changed files with 1,703 additions and 1,257 deletions.
diff --git a/exercises/5.0/src/myworkcell_core/CMakeLists.txt b/exercises/5.0/src/myworkcell_core/CMakeLists.txt
@@ -1,8 +1,8 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(myworkcell_core)
 
-## Add support for C++11, supported in ROS Kinetic and newer
-add_definitions(-std=c++11)
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -41,10 +41,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 ## * In the file package.xml:
 ##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
 ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 ##     but can be declared for certainty nonetheless:
-##     * add a run_depend tag for "message_runtime"
+##     * add a exec_depend tag for "message_runtime"
 ## * In this file (CMakeLists.txt):
 ##   * add "message_generation" and every package in MSG_DEP_SET to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -79,7 +79,7 @@ add_service_files(
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
-  geometry_msgs
+  geometry_msgs  # Or other packages containing msgs
   trajectory_msgs
 )
 
@@ -90,7 +90,7 @@ generate_messages(
 ## To declare and build dynamic reconfigure parameters within this
 ## package, follow these steps:
 ## * In the file package.xml:
-##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
 ## * In this file (CMakeLists.txt):
 ##   * add "dynamic_reconfigure" to
 ##     find_package(catkin REQUIRED COMPONENTS ...)
@@ -108,26 +108,26 @@ generate_messages(
 ###################################
 ## The catkin_package macro generates cmake config files for your package
 ## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
+## INCLUDE_DIRS: uncomment this if your package contains header files
 ## LIBRARIES: libraries you create in this project that dependent projects also need
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES myworkcell_core
- CATKIN_DEPENDS
-   roscpp
-   fake_ar_publisher
-   message_runtime
-   geometry_msgs
-   tf
-   moveit_ros_planning_interface
-   ur5_demo_descartes
-   descartes_trajectory
-   descartes_planner
-   descartes_utilities
-   trajectory_msgs
-   tf_conversions
+   CATKIN_DEPENDS
+     roscpp
+     fake_ar_publisher
+     message_runtime
+     geometry_msgs
+     tf
+     moveit_ros_planning_interface
+     ur5_demo_descartes
+     descartes_trajectory
+     descartes_planner
+     descartes_utilities
+     trajectory_msgs
+     tf_conversions
 #  DEPENDS system_lib
 )
 
@@ -137,8 +137,8 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
 include_directories(
+# include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -182,7 +182,6 @@ target_link_libraries(descartes_node ${catkin_LIBRARIES})
 target_link_libraries(adv_descartes_node ${catkin_LIBRARIES})
 target_link_libraries(adv_myworkcell_node ${catkin_LIBRARIES})
 
-
 #############
 ## Install ##
 #############

diff --git a/exercises/5.0/src/myworkcell_core/package.xml b/exercises/5.0/src/myworkcell_core/package.xml
@@ -1,37 +1,70 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>myworkcell_core</name>
   <version>0.0.0</version>
   <description>The myworkcell_core package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
   <maintainer email="[email protected]">ros-industrial</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>TODO</license>
 
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/myworkcell_core</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
-  <run_depend>roscpp</run_depend>
-  <build_depend>fake_ar_publisher</build_depend>
-  <run_depend>fake_ar_publisher</run_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <depend>fake_ar_publisher</depend>
   <build_depend>message_generation</build_depend>
-  <run_depend>message_runtime</run_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <build_depend>tf</build_depend>
-  <run_depend>tf</run_depend>
-  <build_depend>moveit_ros_planning_interface</build_depend>
-  <run_depend>moveit_ros_planning_interface</run_depend>
-  <build_depend>ur5_demo_descartes</build_depend>
-  <run_depend>ur5_demo_descartes</run_depend>
-  <build_depend>descartes_trajectory</build_depend>
-  <run_depend>descartes_trajectory</run_depend>
-  <build_depend>descartes_planner</build_depend>
-  <run_depend>descartes_planner</run_depend>
-  <build_depend>descartes_utilities</build_depend>
-  <run_depend>descartes_utilities</run_depend>
-  <build_depend>trajectory_msgs</build_depend>
-  <run_depend>trajectory_msgs</run_depend>
-  <build_depend>tf_conversions</build_depend>
-  <run_depend>tf_conversions</run_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>ur5_demo_descartes</depend>
+  <depend>descartes_trajectory</depend>
+  <depend>descartes_planner</depend>
+  <depend>descartes_utilities</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>tf_conversions</depend>
+
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

diff --git a/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp b/exercises/5.0/src/myworkcell_core/src/adv_descartes_node.cpp
@@ -25,7 +25,7 @@ std::vector<double> getCurrentJointState(const std::string& topic)
 EigenSTL::vector_Affine3d makeLine(const Eigen::Vector3d& start, const Eigen::Vector3d& stop, double ds)
 {
   EigenSTL::vector_Affine3d line;
-  
+
   const Eigen::Vector3d travel = stop - start;
   const int steps = std::floor(travel.norm() / ds);
 
@@ -61,7 +61,7 @@ class CartesianPlanner
   {
     // Create a robot model
     model_ = boost::make_shared<ur5_demo_descartes::UR5RobotModel>();
-    
+
     // Define the relevant "frames"
     const std::string robot_description = "robot_description";
     const std::string group_name = "puzzle";
@@ -127,7 +127,8 @@ class CartesianPlanner
     EigenSTL::vector_Affine3d path;
     std::ifstream indata;
 
-    std::string filename = ros::package::getPath("myworkcell_core") + "/config/puzzle_bent.csv";
+    std::string packagePath = ros::package::getPath("myworkcell_core");
+    std::string filename = packagePath + "/config/puzzle_bent.csv";
 
     indata.open(filename);
 
@@ -232,8 +233,8 @@ class CartesianPlanner
     {
       auto p = point;
       TolerancedFrame tool_pt(p);
-      tool_pt.orientation_tolerance.z_lower -= M_PI;
-      tool_pt.orientation_tolerance.z_upper += M_PI;
+      tool_pt.orientation_tolerance.z_lower = -M_PI;
+      tool_pt.orientation_tolerance.z_upper = +M_PI;
 
       boost::shared_ptr<CartTrajectoryPt> pt(new CartTrajectoryPt(wobj_base, wobj_pt, tool_base, tool_pt, 0, M_PI/20.0));
       descartes_path.push_back(pt);

diff --git a/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp b/exercises/5.0/src/myworkcell_core/src/descartes_node.cpp
@@ -117,21 +117,21 @@ class CartesianPlanner
 
     // We assume that our path is centered at (0, 0, 0), so let's define the
     // corners of the AR marker
-    const double side_length = 0.25; // All units are in meters (M)
+    const double side_length = 0.08; // All units are in meters (M)
     const double half_side = side_length / 2.0;
     const double step_size = 0.02;
 
-    Eigen::Vector3d top_left (half_side, half_side, 0);
-    Eigen::Vector3d bot_left (-half_side, half_side, 0);
-    Eigen::Vector3d bot_right (-half_side, -half_side, 0);
-    Eigen::Vector3d top_right (half_side, -half_side, 0);
+    Eigen::Vector3d top_left (-half_side, half_side, 0);
+    Eigen::Vector3d bot_left (-half_side, -half_side, 0);
+    Eigen::Vector3d bot_right (half_side, -half_side, 0);
+    Eigen::Vector3d top_right (half_side, half_side, 0);
 
     // Descartes requires you to guide it in how dense the points should be,
     // so you have to do your own "discretization".
     // NOTE that the makeLine function will create a sequence of points inclusive
     // of the start and exclusive of finish point, i.e. line = [start, stop)
 
-    // Create cartesian path from line-segments
+    // TODO: Add the rest of the cartesian path
     auto segment1 = makeLine(top_left, bot_left, step_size);
     auto segment2 = makeLine(bot_left, bot_right, step_size);
     auto segment3 = makeLine(bot_right, top_right, step_size);
@@ -156,7 +156,7 @@ class CartesianPlanner
 
     for (auto& point : path)
     {
-      // Create a Descartes "cartesian" point with some kind of constraints
+      // TODO: make a Descartes "cartesian" point with some kind of constraints
       descartes_core::TrajectoryPtPtr pt = makeTolerancedCartesianPoint(ref * point);
       descartes_path.push_back(pt);
     }

diff --git a/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp b/exercises/5.0/src/myworkcell_core/src/myworkcell_node.cpp
@@ -18,7 +18,6 @@ class ScanNPlan
   void start(const std::string& base_frame)
   {
     ROS_INFO("Attempting to localize part");
-
     // Localize the part
     myworkcell_core::LocalizePart srv;
     srv.request.base_frame = base_frame;
@@ -32,9 +31,10 @@ class ScanNPlan
     ROS_INFO_STREAM("part localized: " << srv.response);
 
     geometry_msgs::Pose move_target = srv.response.pose;
+    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
 
     // Plan for robot to move to part
-    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
+    move_group.setPoseReferenceFrame(base_frame);
     move_group.setPoseTarget(move_target);
     move_group.move();
 
@@ -67,21 +67,19 @@ int main(int argc, char **argv)
 {
   ros::init(argc, argv, "myworkcell_node");
   ros::NodeHandle nh;
-  ros::NodeHandle private_node_handle("~");
-  ros::AsyncSpinner async_spinner (1);
+  ros::NodeHandle private_node_handle ("~");
+  ros::AsyncSpinner async_spinner(1);
+  async_spinner.start();
 
   ROS_INFO("ScanNPlan node has been initialized");
 
   std::string base_frame;
   private_node_handle.param<std::string>("base_frame", base_frame, "world"); // parameter name, string object reference, default value
 
   ScanNPlan app(nh);
-  ros::Duration(.5).sleep();  // wait for the class to initialize
 
-  async_spinner.start();
+  ros::Duration(.5).sleep();  // wait for the class to initialize
   app.start(base_frame);
 
-  ROS_INFO("ScanNPlan node has been initialized");
-
   ros::waitForShutdown();
 }