Clang formatting and various integration changes

Author: marrts

changes.diff

@@ -1,10 +1,11 @@
 crs:
   general: {}
   motion_planning:
-    pre_move_home: false
+    pre_move_home: true
     home_position:
       joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
-      joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994]
+#      joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994]
+      joint_position: [1.03515, 0.14504, -2.359510, -1.8396, -1.00758, 0.785747]
     process_path:
       tool_speed: 0.1
       offset_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # [px, py, pz, rx, ry, rz] relative to default tool

crs_application/config/main/crs.yaml

@@ -15,7 +15,7 @@ general:
   max_rotation_rate: 3.0
   max_joint_acc: 0.8
   add_approach_and_retreat: true
-  minimum_raster_length: 5
+  minimum_raster_length: 4
   reachable_radius: 1.35
   trajopt_verbose_output: false
   combine_strips: true
@@ -59,7 +59,7 @@ trajopt_surface:
       cost: 10.0
 ompl:
   collision_safety_margin: 0.01
-  planning_time: 120.0
+  planning_time: 240.0
   simplify: true
   range: 0.15
   num_threads: 8

crs_application/config/main/motion_planning/MP_config.yaml

@@ -1,16 +1,17 @@
 general:
   mesh_num_samples: 100000
-  leaf_size: 0.02
+  leaf_size: 0.025
+  part_leaf_size: 0.0175
   part_frame: "part"
   enable_debug_visualizations: True
 icp:
-  max_correspondence_dist: 0.3
+  max_correspondence_dist: 0.08
   max_iter: 15000
   transformation_eps: 1e-12
   rotation_eps: 1e-12  # transformation rotation epsilon in order for an optimization to be considered as having converged to the final solution (epsilon is the cos(angle)
   euclidean_fitness: 1e-12
   ransac_threshold: 0.05
-  use_correspondences: True
+  use_correspondences: False
   use_cog_alignment: False
 sac:
   normal_est_rad: 0.1
@@ -27,7 +28,7 @@ crop_boxes:
     size: [2.0, 2.0, 0.8]
     reverse: True
   box2:
-    xyz: [-0.4, 0.0, 0.56]
+    xyz: [-0.4, 0.0, 0.4]
     size: [0.7, 3.0, 0.8]
     reverse: False # deletes what's inside the box
   box3:

crs_application/config/main/part_localization.yaml

@@ -1,7 +1,7 @@
-scale_factor: 1.2
+scale_factor: 1.25
 plane_dist_threshold: 0.2
 heigth_limits_min: -0.1
 heigth_limits_max: 0.1
 dir_estimation_method: 1       # PLANE_NORMAL: 1, NORMAL_AVGR: 2, POSE_Z_AXIS: 3, USER_DEFINED: 4
 user_dir: [0.0, 0.0, 0.1]      # Used when dir_estimation_method == 4
-view_point: [0.0, 0.0, 10.0]
+view_point: [0.0, 0.0, 10.0]

crs_application/config/main/rework/region_crop.yaml

@@ -1,6 +1,7 @@
 general:
   mesh_num_samples: 100000
   leaf_size: 0.02
+  part_leaf_size: 0.0175
   part_frame: "part"
   enable_debug_visualizations: True
 icp:
@@ -29,4 +30,4 @@ crop_boxes:
   box2:
     xyz: [0.0, -0.875, -0.42]
     size: [2.5, 3.0, 0.2]
-    reverse: False # deletes what's inside the box
+    reverse: False # deletes what's inside the box

crs_application/config/swri/part_localization.yaml

@@ -6,14 +6,11 @@ Panels:
       Expanded:
         - /RobotMotionPreview1/Description Topic1
         - /TF1/Frames1
-        - /MarkerArray1
         - /Scan Poses1/Topic1
         - /Rework Detected Regions1/Namespaces1
         - /Rework Cropped Toolpaths1/Namespaces1
-        - /Selectable Regions1
-        - /MarkerArray2
       Splitter Ratio: 0.44999998807907104
-    Tree Height: 802
+    Tree Height: 775
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -246,7 +243,7 @@ Visualization Manager:
       Axes Radius: 0.009999999776482582
       Class: rviz_default_plugins/PoseArray
       Color: 255; 25; 0
-      Enabled: true
+      Enabled: false
       Head Length: 0.07000000029802322
       Head Radius: 0.029999999329447746
       Name: Scan Poses
@@ -259,7 +256,7 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /crs/scan_poses
-      Value: true
+      Value: false
     - Alpha: 1
       Class: rviz_default_plugins/RobotModel
       Collision Enabled: false
@@ -448,7 +445,7 @@ Visualization Manager:
       Enabled: true
       Name: Process Planning Debug 3
       Namespaces:
-        {}
+        "": true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -472,7 +469,7 @@ Visualization Manager:
       Enabled: true
       Name: Process Planning Debug 2
       Namespaces:
-        {}
+        "": true
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -481,12 +478,12 @@ Visualization Manager:
         Value: /crs/fixed_raster_paths
       Value: true
     - Class: rviz_default_plugins/Axes
-      Enabled: true
+      Enabled: false
       Length: 1
       Name: Axes
       Radius: 0.10000000149011612
       Reference Frame: world
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Name: Rework Detected Regions
@@ -521,24 +518,57 @@ Visualization Manager:
       Show Visual Aids: false
       Value: true
     - Class: rviz_default_plugins/Axes
-      Enabled: true
+      Enabled: false
       Length: 1
       Name: Axes
       Radius: 0.10000000149011612
       Reference Frame: world
-      Value: true
+      Value: false
     - Class: rviz_default_plugins/MarkerArray
       Enabled: true
       Name: MarkerArray
       Namespaces:
-        {}
+        "": true
       Topic:
         Depth: 5
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /crs/original_raster_paths
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /crs/camera/pointcloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -582,41 +612,41 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 3.3550431728363037
+      Distance: 3.247861385345459
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.9376643896102905
-        Y: 0.14379344880580902
-        Z: 1.135255217552185
+        X: -0.344738245010376
+        Y: -0.04360061138868332
+        Z: 0.39920878410339355
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3054007887840271
+      Pitch: 0.3847961723804474
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.4435672760009766
+      Yaw: 3.9713141918182373
     Saved: ~
 Window Geometry:
   ApplicationPanel:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1052
+  Height: 1025
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000297000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003d000003c2000002a00100001cfa000000010100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb00000020004100700070006c00690063006100740069006f006e00500061006e0065006c010000000000000297000001eb00ffffff000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004e3000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000297000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003d000003a7000001ea0100001cfa000000010100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb00000020004100700070006c00690063006100740069006f006e00500061006e0065006c0100000000000002970000020a00ffffff000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004a0000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 3000
-  Y: 0
+  Width: 1853
+  X: 1987
+  Y: 27

crs_application/resources/crs.rviz

@@ -485,7 +485,6 @@ common::ActionResult ProcessExecutionManager::moveRobotIfNotAtStart(const trajec
 common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_msgs::msg::JointTrajectory& traj)
 {
   using namespace control_msgs::action;
-  static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
 
   common::ActionResult res = false;
 
@@ -513,7 +512,6 @@ ProcessExecutionManager::execSurfaceTrajectory(const cartesian_trajectory_msgs::
                                                const trajectory_msgs::msg::JointTrajectory& joint_traj)
 {
   using namespace control_msgs::action;
-  static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
 
   // rclcpp::Duration traj_dur(traj.points.back().time_from_start);
   common::ActionResult res = false;

crs_application/src/task_managers/process_execution_manager.cpp

@@ -187,7 +187,6 @@ common::ActionResult ScanAcquisitionManager::moveRobot()
   freespace_motion_request->goal_pose = scan_poses_.at(scan_index_);
   freespace_motion_request->execute = true;
 
-
   auto result_future = call_freespace_motion_client_->async_send_request(freespace_motion_request);
 
   std::future_status status = result_future.wait_for(std::chrono::duration<double>(WAIT_MOTION_COMPLETION));

crs_application/src/task_managers/scan_acquisition_manager.cpp

@@ -368,7 +368,7 @@ void crs_motion_planning::rasterStripsToMarkerArray(const geometry_msgs::msg::Po
     arrow.color.b = color[3];
     arrow.scale.x = 0.0025;
     arrow.scale.y = 0.005;
-    arrow.scale.z = 0.005;
+    arrow.scale.z = 0.025;
     arrow.id = arrow_count++;
 
     arrow.pose = tf2::toMsg(Eigen::Isometry3d::Identity());