Edge Processing

godel_process_planning/src/blend_process_planning.cpp

@@ -153,6 +153,9 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin
     return false;
   }
 
+  // Enable Collision Checks
+  blend_model_->setCheckCollisions(true);
+
   // Transform process path from geometry msgs to descartes points
   DescartesTraj process_points = toDescartesTraj(req.path.poses, req.params);
 
@@ -231,6 +234,17 @@ bool ProcessPlanningManager::handleBlendPlanning(godel_msgs::BlendProcessPlannin
                                solved_path.end() - from_process.size());
     trajectory_msgs::JointTrajectory process = toROSTrajectory(process_part, *blend_model_);
 
+    for (std::size_t i = 0; i < process.points.size(); ++i)
+    {
+      const auto& pt = process.points[i];
+      if (!blend_model_->isValid(pt.positions))
+      {
+        ROS_WARN_STREAM("Position in blending path (" << i << ") invalid: Joint limit or collision detected\n");
+        return false;
+      }
+    }
+
+
     // Fill in result trajectories
     res.plan.trajectory_process = process;
     res.plan.trajectory_approach = approach;

godel_process_planning/src/common_utils.cpp

@@ -229,28 +229,6 @@ trajectory_msgs::JointTrajectory godel_process_planning::planFreeMove(
     moveit::core::RobotModelConstPtr moveit_model, const std::vector<double>& start,
     const std::vector<double>& stop)
 {
-  // Using a mutable model, turns collision checking on for just the
-  // period of this function. Functions called in this function may
-  // throw exceptions and this makes sure the system state is always
-  // valid
-  struct CollisionsGuard
-  {
-    CollisionsGuard(descartes_core::RobotModel& model) : model_(model)
-    {
-      model.setCheckCollisions(true);
-      ROS_WARN_STREAM("Enabling collision");
-    }
-    ~CollisionsGuard()
-    {
-      model_.setCheckCollisions(false);
-      ROS_WARN_STREAM("Disable collision");
-    }
-    descartes_core::RobotModel& model_;
-  };
-
-  // Create gaurd to enable collisions only for this function
-  CollisionsGuard guard(model);
-
   // Attempt joint interpolated motion
   DescartesTraj joint_approach = createJointPath(start, stop);
 
@@ -281,16 +259,12 @@ std::vector<std::vector<double>>
 godel_process_planning::filterColliding(descartes_core::RobotModel& model,
                                         const std::vector<std::vector<double>>& candidates)
 {
-  model.setCheckCollisions(true);
-
   std::vector<std::vector<double>> results;
 
   for (const auto& c : candidates)
   {
     if (model.isValid(c))
       results.push_back(c);
   }
-
-  model.setCheckCollisions(false);
   return results;
 }

godel_robots/abb/godel_irb2400/abb_irb2400_descartes/src/abb_irb2400_robot_model.cpp

@@ -85,28 +85,9 @@ bool AbbIrb2400RobotModel::getAllIK(const Eigen::Affine3d& pose,
       ROS_DEBUG_STREAM("Original solution size: " << sol.size()
                                                   << ", number of joints: " << num_joints_);
 
-      bool obeys_limits = true;
-      for (unsigned int i = 0; i < sol.size(); i++)
-      {
-        // Add tolerance to limit check
-        if (joint_has_limits_vector_[i] &&
-            ((sol[i] < (joint_min_vector_[i] - JOINT_LIMIT_TOLERANCE)) ||
-             (sol[i] > (joint_max_vector_[i] + JOINT_LIMIT_TOLERANCE))))
-        {
-          // One element of solution is not within limits
-          obeys_limits = false;
-          ROS_DEBUG_STREAM("Not in limits! " << i << " value " << sol[i]
-                                             << " has limit: " << joint_has_limits_vector_[i]
-                                             << "  being  " << joint_min_vector_[i] << " to "
-                                             << joint_max_vector_[i]);
-          break;
-        }
-      }
-
-      if (obeys_limits)
-      {
+      if (isValid(sol))
         joint_poses.push_back(sol);
-      }
+
     }
   }
 

godel_robots/abb/godel_irb2400/godel_irb2400_support/launch/irb2400_blending.launch

@@ -53,7 +53,7 @@
   <group unless="$(arg sim_sensor)">
     <remap from="/depth/points" to="/sensor_point_cloud"/>
     <param name="camera_frame_id" type="string" value="tool0" />
-    <node name="ensenso_publisher_node" pkg="ensenso_publisher" type="ensenso_publisher_node" output="screen">
+    <node name="ensenso_publisher_node" pkg="ensenso_publisher" type="ensenso_publisher_node" output="screen" required="true">
         <param name="FlexView" type="bool" value="true" />
         <param name="FlexViewImages" type="int" value="4" />
     </node>

godel_robots/abb/godel_irb2400/godel_irb2400_support/rviz/irb2400_blending.rviz

@@ -344,6 +344,38 @@ Visualization Manager:
         {}
       Update Interval: 0
       Value: false
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.00499999989
+      Axes Radius: 0.00100000005
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: EdgePoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /edge_visualization
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.00499999989
+      Axes Radius: 0.00100000005
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: false
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: BlendPoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /blend_visualization
+      Unreliable: false
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -362,7 +394,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.96323609
+      Distance: 4.5955658
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -386,12 +418,12 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1176
+  Height: 752
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000001000000000000022b00000452fc0200000002fc0000002800000452000000fb0100001dfa000000020100000003fb000000240052006f0062006f00740042006c0065006e00640069006e006700500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0100000000000001ba0000009500fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c00000004480000001600000000000000000000040f0000045200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000001000000000000022b000002aafc0200000002fc00000028000002aa000000fb0100001dfa000000000100000003fb000000240052006f0062006f00740042006c0065006e00640069006e006700500061006e0065006c0100000000ffffffff000000bf00fffffffb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c0100000000000001ba0000009500fffffffb0000001a0042006c0065006e00640069006e006700500061006e0065006c000000044800000016000000000000000000000414000002aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   RobotBlendingPanel:
     collapsed: false
-  Width: 1600
+  Width: 1605
   X: 0
-  Y: 24
+  Y: 28

godel_surface_detection/include/godel_surface_detection/detection/surface_detection.h

@@ -146,6 +146,7 @@ class SurfaceDetection
   void save_parameters(const std::string& filename);
 
   bool find_surfaces();
+  bool find_surfaces_old();
   std::string get_results_summary();
 
   static void mesh_to_marker(const pcl::PolygonMesh& mesh, visualization_msgs::Marker& marker);