Skip to content

Commit 8f300dd

Browse files
marrtsjrgnicho
authored andcommitted
Clang formatting and various integration changes
1 parent bc20ecd commit 8f300dd

File tree

12 files changed

+45362
-54
lines changed

12 files changed

+45362
-54
lines changed

changes.diff

Lines changed: 45225 additions & 0 deletions
Large diffs are not rendered by default.

crs_application/config/main/crs.yaml

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,11 @@
11
crs:
22
general: {}
33
motion_planning:
4-
pre_move_home: false
4+
pre_move_home: true
55
home_position:
66
joint_names: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]
7-
joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994]
7+
# joint_position: [-0.9644, -1.3617, 2.0724, -0.7108, -0.9640, 0.7994]
8+
joint_position: [1.03515, 0.14504, -2.359510, -1.8396, -1.00758, 0.785747]
89
process_path:
910
tool_speed: 0.1
1011
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/motion_planning/MP_config.yaml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@ general:
1515
max_rotation_rate: 3.0
1616
max_joint_acc: 0.8
1717
add_approach_and_retreat: true
18-
minimum_raster_length: 5
18+
minimum_raster_length: 4
1919
reachable_radius: 1.35
2020
trajopt_verbose_output: false
2121
combine_strips: true
@@ -59,7 +59,7 @@ trajopt_surface:
5959
cost: 10.0
6060
ompl:
6161
collision_safety_margin: 0.01
62-
planning_time: 120.0
62+
planning_time: 240.0
6363
simplify: true
6464
range: 0.15
6565
num_threads: 8

crs_application/config/main/part_localization.yaml

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,17 @@
11
general:
22
mesh_num_samples: 100000
3-
leaf_size: 0.02
3+
leaf_size: 0.025
4+
part_leaf_size: 0.0175
45
part_frame: "part"
56
enable_debug_visualizations: True
67
icp:
7-
max_correspondence_dist: 0.3
8+
max_correspondence_dist: 0.08
89
max_iter: 15000
910
transformation_eps: 1e-12
1011
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)
1112
euclidean_fitness: 1e-12
1213
ransac_threshold: 0.05
13-
use_correspondences: True
14+
use_correspondences: False
1415
use_cog_alignment: False
1516
sac:
1617
normal_est_rad: 0.1
@@ -27,7 +28,7 @@ crop_boxes:
2728
size: [2.0, 2.0, 0.8]
2829
reverse: True
2930
box2:
30-
xyz: [-0.4, 0.0, 0.56]
31+
xyz: [-0.4, 0.0, 0.4]
3132
size: [0.7, 3.0, 0.8]
3233
reverse: False # deletes what's inside the box
3334
box3:
Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
1-
scale_factor: 1.2
1+
scale_factor: 1.25
22
plane_dist_threshold: 0.2
33
heigth_limits_min: -0.1
44
heigth_limits_max: 0.1
55
dir_estimation_method: 1 # PLANE_NORMAL: 1, NORMAL_AVGR: 2, POSE_Z_AXIS: 3, USER_DEFINED: 4
66
user_dir: [0.0, 0.0, 0.1] # Used when dir_estimation_method == 4
7-
view_point: [0.0, 0.0, 10.0]
7+
view_point: [0.0, 0.0, 10.0]

crs_application/config/swri/part_localization.yaml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
general:
22
mesh_num_samples: 100000
33
leaf_size: 0.02
4+
part_leaf_size: 0.0175
45
part_frame: "part"
56
enable_debug_visualizations: True
67
icp:
@@ -29,4 +30,4 @@ crop_boxes:
2930
box2:
3031
xyz: [0.0, -0.875, -0.42]
3132
size: [2.5, 3.0, 0.2]
32-
reverse: False # deletes what's inside the box
33+
reverse: False # deletes what's inside the box

crs_application/resources/crs.rviz

Lines changed: 54 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -6,14 +6,11 @@ Panels:
66
Expanded:
77
- /RobotMotionPreview1/Description Topic1
88
- /TF1/Frames1
9-
- /MarkerArray1
109
- /Scan Poses1/Topic1
1110
- /Rework Detected Regions1/Namespaces1
1211
- /Rework Cropped Toolpaths1/Namespaces1
13-
- /Selectable Regions1
14-
- /MarkerArray2
1512
Splitter Ratio: 0.44999998807907104
16-
Tree Height: 802
13+
Tree Height: 775
1714
- Class: rviz_common/Selection
1815
Name: Selection
1916
- Class: rviz_common/Tool Properties
@@ -246,7 +243,7 @@ Visualization Manager:
246243
Axes Radius: 0.009999999776482582
247244
Class: rviz_default_plugins/PoseArray
248245
Color: 255; 25; 0
249-
Enabled: true
246+
Enabled: false
250247
Head Length: 0.07000000029802322
251248
Head Radius: 0.029999999329447746
252249
Name: Scan Poses
@@ -259,7 +256,7 @@ Visualization Manager:
259256
History Policy: Keep Last
260257
Reliability Policy: Reliable
261258
Value: /crs/scan_poses
262-
Value: true
259+
Value: false
263260
- Alpha: 1
264261
Class: rviz_default_plugins/RobotModel
265262
Collision Enabled: false
@@ -448,7 +445,7 @@ Visualization Manager:
448445
Enabled: true
449446
Name: Process Planning Debug 3
450447
Namespaces:
451-
{}
448+
"": true
452449
Topic:
453450
Depth: 5
454451
Durability Policy: Volatile
@@ -472,7 +469,7 @@ Visualization Manager:
472469
Enabled: true
473470
Name: Process Planning Debug 2
474471
Namespaces:
475-
{}
472+
"": true
476473
Topic:
477474
Depth: 5
478475
Durability Policy: Volatile
@@ -481,12 +478,12 @@ Visualization Manager:
481478
Value: /crs/fixed_raster_paths
482479
Value: true
483480
- Class: rviz_default_plugins/Axes
484-
Enabled: true
481+
Enabled: false
485482
Length: 1
486483
Name: Axes
487484
Radius: 0.10000000149011612
488485
Reference Frame: world
489-
Value: true
486+
Value: false
490487
- Class: rviz_default_plugins/MarkerArray
491488
Enabled: true
492489
Name: Rework Detected Regions
@@ -521,24 +518,57 @@ Visualization Manager:
521518
Show Visual Aids: false
522519
Value: true
523520
- Class: rviz_default_plugins/Axes
524-
Enabled: true
521+
Enabled: false
525522
Length: 1
526523
Name: Axes
527524
Radius: 0.10000000149011612
528525
Reference Frame: world
529-
Value: true
526+
Value: false
530527
- Class: rviz_default_plugins/MarkerArray
531528
Enabled: true
532529
Name: MarkerArray
533530
Namespaces:
534-
{}
531+
"": true
535532
Topic:
536533
Depth: 5
537534
Durability Policy: Volatile
538535
History Policy: Keep Last
539536
Reliability Policy: Reliable
540537
Value: /crs/original_raster_paths
541538
Value: true
539+
- Alpha: 1
540+
Autocompute Intensity Bounds: true
541+
Autocompute Value Bounds:
542+
Max Value: 10
543+
Min Value: -10
544+
Value: true
545+
Axis: Z
546+
Channel Name: intensity
547+
Class: rviz_default_plugins/PointCloud2
548+
Color: 255; 255; 255
549+
Color Transformer: RGB8
550+
Decay Time: 0
551+
Enabled: false
552+
Invert Rainbow: false
553+
Max Color: 255; 255; 255
554+
Max Intensity: 4096
555+
Min Color: 0; 0; 0
556+
Min Intensity: 0
557+
Name: PointCloud2
558+
Position Transformer: XYZ
559+
Selectable: true
560+
Size (Pixels): 3
561+
Size (m): 0.009999999776482582
562+
Style: Flat Squares
563+
Topic:
564+
Depth: 5
565+
Durability Policy: Volatile
566+
History Policy: Keep Last
567+
Reliability Policy: Reliable
568+
Value: /crs/camera/pointcloud
569+
Use Fixed Frame: true
570+
Use rainbow: true
571+
Value: false
542572
Enabled: true
543573
Global Options:
544574
Background Color: 48; 48; 48
@@ -582,41 +612,41 @@ Visualization Manager:
582612
Views:
583613
Current:
584614
Class: rviz_default_plugins/Orbit
585-
Distance: 3.3550431728363037
615+
Distance: 3.247861385345459
586616
Enable Stereo Rendering:
587617
Stereo Eye Separation: 0.05999999865889549
588618
Stereo Focal Distance: 1
589619
Swap Stereo Eyes: false
590620
Value: false
591621
Focal Point:
592-
X: -0.9376643896102905
593-
Y: 0.14379344880580902
594-
Z: 1.135255217552185
622+
X: -0.344738245010376
623+
Y: -0.04360061138868332
624+
Z: 0.39920878410339355
595625
Focal Shape Fixed Size: true
596626
Focal Shape Size: 0.05000000074505806
597627
Invert Z Axis: false
598628
Name: Current View
599629
Near Clip Distance: 0.009999999776482582
600-
Pitch: 0.3054007887840271
630+
Pitch: 0.3847961723804474
601631
Target Frame: <Fixed Frame>
602632
Value: Orbit (rviz)
603-
Yaw: 3.4435672760009766
633+
Yaw: 3.9713141918182373
604634
Saved: ~
605635
Window Geometry:
606636
ApplicationPanel:
607637
collapsed: false
608638
Displays:
609639
collapsed: false
610-
Height: 1052
640+
Height: 1025
611641
Hide Left Dock: false
612642
Hide Right Dock: true
613-
QMainWindow State: 000000ff00000000fd000000040000000000000297000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003d000003c2000002a00100001cfa000000010100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb00000020004100700070006c00690063006100740069006f006e00500061006e0065006c010000000000000297000001eb00ffffff000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004e3000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
643+
QMainWindow State: 000000ff00000000fd000000040000000000000297000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fc0000003d000003a7000001ea0100001cfa000000010100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb00000020004100700070006c00690063006100740069006f006e00500061006e0065006c0100000000000002970000020a00ffffff000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004a0000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
614644
Selection:
615645
collapsed: false
616646
Tool Properties:
617647
collapsed: false
618648
Views:
619649
collapsed: true
620-
Width: 1920
621-
X: 3000
622-
Y: 0
650+
Width: 1853
651+
X: 1987
652+
Y: 27

crs_application/src/task_managers/process_execution_manager.cpp

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -485,7 +485,6 @@ common::ActionResult ProcessExecutionManager::moveRobotIfNotAtStart(const trajec
485485
common::ActionResult ProcessExecutionManager::execTrajectory(const trajectory_msgs::msg::JointTrajectory& traj)
486486
{
487487
using namespace control_msgs::action;
488-
static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
489488

490489
common::ActionResult res = false;
491490

@@ -513,7 +512,6 @@ ProcessExecutionManager::execSurfaceTrajectory(const cartesian_trajectory_msgs::
513512
const trajectory_msgs::msg::JointTrajectory& joint_traj)
514513
{
515514
using namespace control_msgs::action;
516-
static const double GOAL_ACCEPT_TIMEOUT_PERIOD = 1.0;
517515

518516
// rclcpp::Duration traj_dur(traj.points.back().time_from_start);
519517
common::ActionResult res = false;

crs_application/src/task_managers/scan_acquisition_manager.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -187,7 +187,6 @@ common::ActionResult ScanAcquisitionManager::moveRobot()
187187
freespace_motion_request->goal_pose = scan_poses_.at(scan_index_);
188188
freespace_motion_request->execute = true;
189189

190-
191190
auto result_future = call_freespace_motion_client_->async_send_request(freespace_motion_request);
192191

193192
std::future_status status = result_future.wait_for(std::chrono::duration<double>(WAIT_MOTION_COMPLETION));

crs_motion_planning/src/path_processing_utils.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -368,7 +368,7 @@ void crs_motion_planning::rasterStripsToMarkerArray(const geometry_msgs::msg::Po
368368
arrow.color.b = color[3];
369369
arrow.scale.x = 0.0025;
370370
arrow.scale.y = 0.005;
371-
arrow.scale.z = 0.005;
371+
arrow.scale.z = 0.025;
372372
arrow.id = arrow_count++;
373373

374374
arrow.pose = tf2::toMsg(Eigen::Isometry3d::Identity());

0 commit comments

Comments
 (0)