From 3afeaa2f4a6e51a7f0ed8898d7d6809d95fac8b4 Mon Sep 17 00:00:00 2001 From: Jonathan Hechtbauer Date: Tue, 15 May 2018 12:38:09 +0200 Subject: [PATCH] Add param for scan move vel scaling --- godel_msgs/msg/RobotScanParameters.msg | 1 + .../src/widgets/robot_scan_configuration.cpp | 2 ++ .../src/widgets/robot_scan_configuration.ui | 31 ++++++++++++++++--- .../config/robot_scan.yaml | 1 + .../config/robot_scan.yaml | 1 + .../src/options/robot_scan_configuration.cpp | 2 ++ .../src/uis/robot_scan_configuration.ui | 31 ++++++++++++++++--- .../src/scan/robot_scan.cpp | 4 +++ 8 files changed, 65 insertions(+), 8 deletions(-) diff --git a/godel_msgs/msg/RobotScanParameters.msg b/godel_msgs/msg/RobotScanParameters.msg index 1f724819..72ea7df4 100644 --- a/godel_msgs/msg/RobotScanParameters.msg +++ b/godel_msgs/msg/RobotScanParameters.msg @@ -1,5 +1,6 @@ # path planning string group_name +float64 move_scan_vel_scaling string home_position string world_frame string tcp_frame diff --git a/godel_plugins/src/widgets/robot_scan_configuration.cpp b/godel_plugins/src/widgets/robot_scan_configuration.cpp index f85c1087..79a01a8b 100644 --- a/godel_plugins/src/widgets/robot_scan_configuration.cpp +++ b/godel_plugins/src/widgets/robot_scan_configuration.cpp @@ -82,6 +82,7 @@ void godel_plugins::RobotScanConfigWidget::update_gui_fields() ui_.LineEditWorldFrame->setText(QString::fromStdString(params_.world_frame)); ui_.LineEditTcpFrame->setText(QString::fromStdString(params_.tcp_frame)); ui_.LineEditGroupName->setText(QString::fromStdString(params_.group_name)); + ui_.LineEditMoveScanVelScaling->setText(QString::number(params_.move_scan_vel_scaling)); ui_.CheckBoxStopOnPlanningError->setChecked(params_.stop_on_planning_error); world_to_obj_pose_widget_->set_values(params_.world_to_obj_pose); @@ -102,6 +103,7 @@ void godel_plugins::RobotScanConfigWidget::update_internal_values() params_.world_frame = ui_.LineEditWorldFrame->text().toStdString(); params_.tcp_frame = ui_.LineEditTcpFrame->text().toStdString(); params_.group_name = ui_.LineEditGroupName->text().toStdString(); + params_.move_scan_vel_scaling = ui_.LineEditMoveScanVelScaling->text().toDouble(); params_.stop_on_planning_error = ui_.CheckBoxStopOnPlanningError->isChecked(); tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(), params_.world_to_obj_pose); diff --git a/godel_plugins/src/widgets/robot_scan_configuration.ui b/godel_plugins/src/widgets/robot_scan_configuration.ui index 1ea08119..2cf6bbdd 100644 --- a/godel_plugins/src/widgets/robot_scan_configuration.ui +++ b/godel_plugins/src/widgets/robot_scan_configuration.ui @@ -7,7 +7,7 @@ 0 0 540 - 563 + 583 @@ -105,7 +105,7 @@ 30 40 281 - 441 + 461 @@ -299,13 +299,36 @@ + + + Move Scan Vel Scaling + + + + + + + Qt::ImhDigitsOnly + + + + + + + + + 8 + + + + Stop On Plan Error - + @@ -341,7 +364,7 @@ 33 - 500 + 520 491 33 diff --git a/godel_robots/abb/godel_irb1200/godel_irb1200_support/config/robot_scan.yaml b/godel_robots/abb/godel_irb1200/godel_irb1200_support/config/robot_scan.yaml index 9b0af1c4..25c291b3 100644 --- a/godel_robots/abb/godel_irb1200/godel_irb1200_support/config/robot_scan.yaml +++ b/godel_robots/abb/godel_irb1200/godel_irb1200_support/config/robot_scan.yaml @@ -1,5 +1,6 @@ robot_scan: group_name: manipulator_asus + move_scan_vel_scaling: 1 home_position: home_asus world_frame: world_frame tcp_frame: kinect2_move_frame diff --git a/godel_robots/abb/godel_irb2400/godel_irb2400_support/config/robot_scan.yaml b/godel_robots/abb/godel_irb2400/godel_irb2400_support/config/robot_scan.yaml index 9b0af1c4..25c291b3 100644 --- a/godel_robots/abb/godel_irb2400/godel_irb2400_support/config/robot_scan.yaml +++ b/godel_robots/abb/godel_irb2400/godel_irb2400_support/config/robot_scan.yaml @@ -1,5 +1,6 @@ robot_scan: group_name: manipulator_asus + move_scan_vel_scaling: 1 home_position: home_asus world_frame: world_frame tcp_frame: kinect2_move_frame diff --git a/godel_simple_gui/src/options/robot_scan_configuration.cpp b/godel_simple_gui/src/options/robot_scan_configuration.cpp index 587ef31a..a903cd41 100644 --- a/godel_simple_gui/src/options/robot_scan_configuration.cpp +++ b/godel_simple_gui/src/options/robot_scan_configuration.cpp @@ -40,6 +40,7 @@ void godel_simple_gui::RobotScanConfigWidget::update_display_fields() ui_->LineEditWorldFrame->setText(QString::fromStdString(params_.world_frame)); ui_->LineEditTcpFrame->setText(QString::fromStdString(params_.tcp_frame)); ui_->LineEditGroupName->setText(QString::fromStdString(params_.group_name)); + ui_->LineEditMoveScanVelScaling->setText(QString::number(params_.move_scan_vel_scaling)); ui_->CheckBoxStopOnPlanningError->setChecked(params_.stop_on_planning_error); world_to_obj_pose_widget_->set_values(params_.world_to_obj_pose); @@ -60,6 +61,7 @@ void godel_simple_gui::RobotScanConfigWidget::update_internal_fields() params_.world_frame = ui_->LineEditWorldFrame->text().toStdString(); params_.tcp_frame = ui_->LineEditTcpFrame->text().toStdString(); params_.group_name = ui_->LineEditGroupName->text().toStdString(); + params_.move_scan_vel_scaling = ui_->LineEditMoveScanVelScaling->text().toDouble(); params_.stop_on_planning_error = ui_->CheckBoxStopOnPlanningError->isChecked(); tf::poseTFToMsg(world_to_obj_pose_widget_->get_values(), params_.world_to_obj_pose); diff --git a/godel_simple_gui/src/uis/robot_scan_configuration.ui b/godel_simple_gui/src/uis/robot_scan_configuration.ui index 75558fb6..824c593a 100644 --- a/godel_simple_gui/src/uis/robot_scan_configuration.ui +++ b/godel_simple_gui/src/uis/robot_scan_configuration.ui @@ -7,7 +7,7 @@ 0 0 540 - 563 + 583 @@ -105,7 +105,7 @@ 30 40 281 - 441 + 461 @@ -299,13 +299,36 @@ + + + Move Scan Vel Scaling + + + + + + + Qt::ImhDigitsOnly + + + + + + + + + 8 + + + + Stop On Plan Error - + @@ -341,7 +364,7 @@ 33 - 500 + 520 491 33 diff --git a/godel_surface_detection/src/scan/robot_scan.cpp b/godel_surface_detection/src/scan/robot_scan.cpp index 87f889eb..d9710eb2 100644 --- a/godel_surface_detection/src/scan/robot_scan.cpp +++ b/godel_surface_detection/src/scan/robot_scan.cpp @@ -55,6 +55,7 @@ RobotScan::RobotScan() { params_.group_name = "manipulator_asus"; + params_.move_scan_vel_scaling = 1; params_.home_position = "home"; params_.world_frame = "world_frame"; params_.tcp_frame = "kinect2_move_frame"; @@ -75,6 +76,7 @@ RobotScan::RobotScan() bool RobotScan::init() { move_group_ptr_ = MoveGroupPtr(new moveit::planning_interface::MoveGroupInterface(params_.group_name)); + move_group_ptr_->setMaxVelocityScalingFactor(params_.move_scan_vel_scaling); move_group_ptr_->setEndEffectorLink(params_.tcp_frame); move_group_ptr_->setPoseReferenceFrame(params_.world_frame); move_group_ptr_->setPlanningTime(PLANNING_TIME); @@ -97,6 +99,7 @@ bool RobotScan::load_parameters(const std::string& filename) // otherwise load from parameters ros::NodeHandle nh("~/robot_scan"); return loadParam(nh, "group_name", params_.group_name) && + loadParam(nh, "move_scan_vel_scaling", params_.move_scan_vel_scaling) && loadParam(nh, "home_position", params_.home_position) && loadParam(nh, "world_frame", params_.world_frame) && loadParam(nh, "tcp_frame", params_.tcp_frame) && @@ -361,6 +364,7 @@ bool RobotScan::create_scan_trajectory(std::vector& scan_po scan_poses.push_back(pose); } + move_group_ptr_->setMaxVelocityScalingFactor(params_.move_scan_vel_scaling); move_group_ptr_->setEndEffectorLink(params_.tcp_frame); return true;