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;