Pr-add handeye calibration rviz plugin #4

RoboticsYY · 2020-06-06T13:32:01Z

This PR intends to add a handeye calibration rviz plugin. Any requested changes of MoveIt PR#1560 will be addressed here. For more info about the discussion can refer to MoveIt issue#1070.

RoboticsYY · 2020-06-06T13:35:11Z

Copy @felixvd 's comments here:

@RoboticsYY As I mentioned in #1070 already, I love this effort. Great job. I don't know where best to put the discussion, but as we found one bug in the code of this PR, I'll add comments as I go. Some of these are mentioned in #1070. Sorry for the duplicates. Other comments:

Is the "Marker Size" in the Target tab referring to the size of one marker or the marker board? If it is possible to add a hover tooltip, this would be great to disambiguate (new users have much simpler questions than this).
Our FOV is not displayed. Do we need to add anything to the scene? We tried adding the target_calibration/camera/raw topic as a display to Rviz, but moving its window around caused Rviz to crash.
The marker parameters need to be set up for the detection to work. It would be very convenient to be able to save and load the parameters.
One could save the parameters by saving the Rviz config file, but when reopening Rviz, the list of Planning Groups is empty. This can be fixed by restarting the plugin, but this resets it fully, discarding the marker parameters and other settings. Can the list of planning groups be reloaded upon clicking on it, maybe?
Changing the image topic sometimes crashes Rviz. We could reproduce it a few times by saving the Rviz config with the plugin open and the image topics set, then reopening Rviz, closing the plugin, opening the plugin and setting the image topic. The error message refers to a mutex lock. I don't know if it is related to this code or an issue with Rviz.

RoboticsYY · 2020-06-06T13:38:32Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+
+  // Initialize handeye solver plugins
+  std::vector<std::string> plugins;
+  if (loadSolverPlugin(plugins))


Copy @felixvd 's comments:

This does not seem to populate the list when the plugin is part of the Rviz config file and is loaded when Rviz starts up. Maybe move this into a function that is called upon clicking the planning group dropdown box.

I've looked into this. I do think the list is populated when the plugin is loaded from a config--that is, all the options are there. However, the solver that was selected and saved to the config doesn't get re-selected. It just loads the first one, which happens to be the Daniilidis solver. Is that the behavior you were referring to @felixvd ? (I have a fix here, which will be part of a PR to @RoboticsYY's branch soon.)

It's been too long for me to remember, but it sounds like it might have been.

RoboticsYY · 2020-06-06T13:43:22Z

...ibration_rviz_plugin/include/moveit/handeye_calibration_rviz_plugin/handeye_control_widget.h

+  // **************************************************************
+
+  mhc::SensorMountType sensor_mount_type_;
+  std::map<std::string, std::string> frame_names_;


Copy @felixvd 's comments:

This variable should be initialized, otherwise the "Save Camera Pose" button gives an "Empty Frame" error if the user left the "Sensor Mount Type" dropdown box on its default setting "Eye-to-Hand" without clicking on it.

I don't mind if it is done here as in the suggestion of in the cpp along with the rest here, but this took some error to find and was a confusing error.

Suggested change

std::map<std::string, std::string> frame_names_;

std::map<std::string, std::string> frame_names_ = "base";

This comment was really confusing to me until I found the original and realized it was from a different line. I've added an initialization for from_frame_tag_ to my PR.

RoboticsYY · 2020-06-06T13:46:05Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+
+  if (from_frame.empty() || to_frame.empty())
+  {
+    QMessageBox::warning(this, tr("Empty Frame Name"), tr("Make sure you have set correct frame names."));


Copy @felixvd 's comments here:

This error message could have been more helpful. Can you be more verbose?

RoboticsYY · 2020-06-06T13:47:20Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+
+  auto_execute_btn_ = new QPushButton("Execute");
+  auto_execute_btn_->setMinimumHeight(35);
+  auto_execute_btn_->setToolTip("Pause the auto calibration process");


Copy @felixvd 's comments here:

This tooltip seems to be outdated

RoboticsYY · 2020-06-06T13:48:06Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+  auto_cal_layout->addLayout(auto_btns_layout);
+  auto_plan_btn_ = new QPushButton("Plan");
+  auto_plan_btn_->setMinimumHeight(35);
+  auto_plan_btn_->setToolTip("Start or resume auto calibration process");


Copy @felixvd 's comments here:

Same with this one.

RoboticsYY · 2020-06-06T13:54:40Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+  layout->addLayout(calib_layout);
+
+  // Calibration progress
+  auto_progress_ = new ProgressBarWidget(this);


Copy @felixvd 's comments here:

Can this be initialized to 0% somehow? It looks like is loading and complete upon first opening.

@JStech IIRC when first opening the plugin, the bar is filled and animated, and does not look like it is displaying "0%". That seemed unintuitive.

Here's what's going on (from the Qt docs):

If minimum and maximum both are set to 0, the bar shows a busy indicator instead of a percentage of steps. This is useful, for example, when using QNetworkAccessManager to download items when they are unable to determine the size of the item being downloaded.

The best fix I could come up with is to disable the bar when the max is 0, which grays it out:

RoboticsYY · 2020-06-06T13:57:42Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+  layout->addWidget(auto_progress_);
+
+  // Pose sample tree view area
+  QGroupBox* sample_group = new QGroupBox("Pose_sample");


Copy @felixvd 's comments here:

Suggested change

layout->addWidget(auto_progress_);

// Pose sample tree view area

QGroupBox* sample_group = new QGroupBox("Pose_sample");

QGroupBox* sample_group = new QGroupBox("Pose Samples");

RoboticsYY · 2020-06-06T13:58:31Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+
+  std::ostringstream ss;
+
+  QStandardItem* child_1 = new QStandardItem("bTe");


Copy @felixvd 's comments here:

Do bTe and cTo mean "base_to_endeffector" and "camera_to_object"? I would rename them everywhere for readability. The current names are very cryptic.

RoboticsYY · 2020-06-06T13:59:21Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+  }
+}
+
+void ControlTabWidget::resetSampleBtnClicked(bool clicked)


Copy @felixvd 's comments here:

Redundant comment, but I would rename this to "clearSamplesBtnClicked" and would appreciate a real "resetSampleBtnClicked" function that removes only a single sample from the tree.

RoboticsYY · 2020-06-06T14:00:06Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp

+  }
+  else
+  {
+    ROS_ERROR_STREAM_NAMED(LOGNAME, "No available handeye calibration solver instance.");


Copy @felixvd 's comments here:

I don't know if is this line that should be changed, but currently the solving pipeline fails silently, and the user does not know if there is an issue. At some point there should give an error popup instead of just a silent error stream.

JStech · 2020-06-08T02:52:19Z

I was able to launch this and use it today. I collected a sequence of poses, but I couldn't figure out how to actually calculate and export the calibration. I was also unable to get the auto calibration to work. It raised an error asking whether move_group was running or something--I should've noted it, but I didn't and I'm not in the office now. It might have been something wrong in my config, although I was able to move the arm via the MoveIt RViz plugin.

At some point, also, the camera-to-target pose ("cTo" in the list of samples) was "x: 529649, y: -327991, z: 6.24729e+06". I'm guessing it was probably a bug in the OpenCV ArUco package, but I thought I should report it.

Finally, I have a few suggestions about the text that's used on some of the labels and buttons. I'll put them in a PR to this PR.

I expect to be able to put more time into this this week.

RoboticsYY · 2020-06-08T15:04:29Z

@JStech Thanks for testing and feeding back!

I collected a sequence of poses, but I couldn't figure out how to actually calculate and export the calibration.

When the number of samples exceeds 5, the calculation happens each time a new sample is taken. I think it's a little bit confused as it happens in the background. When enough samples are taken, can click save camera pose button to save the result in a launch file. Then this launch file can be re-used to publish a static tf transform of the camera pose.

I was also unable to get the auto calibration to work. It raised an error asking whether move_group was running or something--I should've noted it, but I didn't and I'm not in the office now. It might have been something wrong in my config, although I was able to move the arm via the MoveIt RViz plugin.

I noticed from the photo that the move_group used endeffector. Maybe it's the reason of failure, a move_group of the arm probably can solve the problem.

At some point, also, the camera-to-target pose ("cTo" in the list of samples) was "x: 529649, y: -327991, z: 6.24729e+06". I'm guessing it was probably a bug in the OpenCV ArUco package, but I thought I should report it.

I think this wrong result mostly comes from OpenCV 3.2, which is the default version on Ubuntu 18.04. I have provided a new PR #5 to fix this by installing OpenCV 3.4 through Debian packages.

In addition, the target detection result could be debugged visually through the TF frame of Rviz and FOV. If adding the TF tree to Rviz display panel, the coordinate frame of camera should show up. Another useful tool is the FOV display. If adding a marker array to Rviz display panel and selecting proper topic, the FOV of the camera should show up. And the coordinate frame of the target should locate within the FOV. There are sliders on the context widget to set the initial guess pose of the camera. The FOV is generated from the camera intrinsic parameters, thus a correct camera_info topic must be chosen at the target widget.

JStech · 2020-06-10T22:24:12Z

OK, I got a calibration! Thank you!

A couple things. I had to debug a stupid mistake on my part (apparently I never did a rosdep install and so I didn't have the Python handeye package installed). In the process, I added throttles to two messages in handeye_target_aruco.cpp (at lines 114 and 193), so I could see the other terminal output. I'd recommend making that change, although maybe I'll just open another PR for that, since it's not really part of this one.

I was able to use the auto calibration to plan poses when I switched the Planning Group option on the Calibrate tab. (Great feature, by the way--thanks for implementing that.) However, it still occasionally would fail to move the arm, and then would give me the error, "Please check if move_group is started or there are recorded joint states." If I moved the arm using the MotionPlanning panel, it would usually start working again.

I was also unable to get the FOV display. I added a MarkerArray display, and the topics available are /move_group/display_contacts, /move_group/display_cost_sources, /move_group/display_grasp_markers, and /rviz_visual_tools. I did have the ON/OFF set to on (well, I tried both, actually).

I had issues with the cTo pose diverging again, so I upgraded to OpenCV 3.4.

JStech · 2020-06-11T03:27:07Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp

+
+  target_params_["marker_border"]->setText(QString("1"));
+  target_params_["marker_border"]->setValidator(new QIntValidator(1, 4));
+  layout_left_top->addRow("Marker border (bits)", target_params_["marker_border"]);


It occurs to me (as I'm working on adding a plugin for a ChArUco board: #7) that these parameters are somewhat specific to the ArUco board. Would it be possible to have target plugins define the parameters and validations?

Great idea! I think both of the aruco and charuco board needs two double parameter values, but actually the meaning of the values are different. For the aruco, they are maker size and marker spacing. And for the charuco, they are square size and marker size. And obviously, it's difficult to list all of the possibilities of different kind of board, because the parameters could be very different. I think it's possible to let the target plugin return a list of parameters and types, and the GUI components are initialized from this list. Do you think that would be better?

Yes, that seems like the best way to handle it. I guess the HandEyeTargetBase class should similarly be updated to have more flexibility in the inputs to intialize, setTargetIntrinsicParams, and setTargetDimensions. Maybe all that should go in a different PR.

JStech · 2020-06-11T03:27:59Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_target_widget.cpp

+
+  target_real_dims_["marker_dist_real"]->setText("0.0066");
+  target_real_dims_["marker_dist_real"]->setValidator(new QDoubleValidator(0, 2, 4));
+  layout_left_bottom->addRow("Marker spacing (m)", target_real_dims_["marker_dist_real"]);


This parameter, also, doesn't quite work for a ChArUco board

RoboticsYY · 2020-06-11T15:06:51Z

I was also unable to get the FOV display. I added a MarkerArray display, and the topics available are /move_group/display_contacts, /move_group/display_cost_sources, /move_group/display_grasp_markers, and /rviz_visual_tools. I did have the ON/OFF set to on (well, I tried both, actually).

I think I forgot to mention that the FOV is using the /rviz_visual_tools topic. Since the FOV mesh is with respect to the sensor base frame (usually camera_link). So it needs a transform between the sensor frame and world frame to show up in 3D. Some extra settings are required. On the Target frame, a proper CameraInfo topic need to be specified. On the context widget, if the sensor configuration is chosen as "eye-to-hand", the sensor_frame and robot base frame need to be specified before the FOV shows up. If the configuration is chosen as "eye-in-hand", the sensor_frame and end-effector frame need to be specified.

JStech · 2020-06-22T20:37:56Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_context_widget.cpp

+
+      if (frame_source_ == CAMERA_FRAME)
+        if (index != std::string::npos)
+          addItem(QString(name.c_str()));


This is requiring that the camera coordinate frame have the string "camera" in it, correct? That seems too restrictive (in fact, just ran into this with a client using a camera node that didn't include "camera" in the frame). Maybe the filter here should also just be that the frame is not a robot link?

I like this sort of default. Can we have the frames with "camera" listed at the top, maybe even as duplicates and separated by a "---" entry, and then list all the frames (except the robot links)?

A tooltip explaining which frame should go in here would also be very helpful.

tandronescu · 2020-06-30T18:22:06Z

Hi, I am interested in using this tool as well. Amazing effort so far, I think this could be immensely useful. I built this branch https://github.com/JStech/moveit_calibration/tree/charuco-with-rviz for now to get it up and running. I'll point out any bugs or issues I find on this PR. Thanks!

tandronescu · 2020-06-30T18:29:27Z

moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_calibration_gui.cpp

+
+  tab_control_ = new ControlTabWidget();
+  tab_control_->setTFTool(tf_tools_);
+  connect(tab_context_, SIGNAL(sensorMountTypeChanged(int)), tab_control_, SLOT(UpdateSensorMountType(int)));


Minor thing, but I believe this will not get changed if the user simply leaves the default sensor mount type as "eye-to-hand". I had to hunt this down since eventually I would run into the "Invalid key used for reading the frame names." error because from_frame_tag_ was never being set.

I had to toggle the sensor mount type to get around this.

I believe I've fixed this in this branch by calling the sensor mount update when the panel is loaded. If you could give it a try and let me know if it's fixed, I'd appreciate it.

Appears to be fixed.

tandronescu · 2020-07-08T21:55:11Z

Possible bug in calibrate tab. If you load a saved joint state file and are doing an auto calibration, pressing plan then execute too quickly will freeze rviz. This requires a restart. If you wait some time for the planning animation to complete, this issue does not arise.

I am able to consistently reproduce this, but it could be something specific to my moveit config setup if no one else can reproduce this.

JStech

I think this is ready to go. Are there any blocking issues I could help address?

RoboticsYY · 2020-07-29T06:14:13Z

Hi @JStech, there are still multiple comments from @felixvd I haven't addressed yet. Can you help on some of them? It will be much helpful.

JStech · 2020-08-05T04:46:18Z

I've addressed what I could in this PR. There are still some hiccups in the auto calibration process that could use some work, but it's usable as-is. I think we should merge this soon, so that the master branch is generally useful, and create issues from any remaining comments.

davetcoleman · 2020-08-12T14:50:30Z

As the original PR for this was opened over a year ago I think we should merge this now (there's been a lot of great work put into this) and any remaining concerns from e.g. @felixvd be turned into issues / feature requests.

I keep hearing everyone is using a non-master branch of this repo, which is a bad practice to follow and a strong sign we should just merge it in because ppl are using it.

tandronescu · 2020-08-12T16:15:58Z

+1 On merging current PR into master. I've been using this tool and have had to jump between branches making the version control more difficult.

JStech

Thanks, @RoboticsYY , for pulling my changes. I think this is ready to go now--the "[WIP]" can be removed. I've opened #15 to discuss how we want to update the GUI to separate out the different functions currently wrapped up in the "Take Sample" button.

I'll also update the tutorial to match.

RoboticsYY · 2020-08-14T06:00:05Z

I think most comments have been addressed. At current stage, this package can be compiled and run as a package along with MoveIt with CI build passed. Thus I agree that it would be better to merge this PR first and then continue to make amends to GUI in other PRs.

Thanks @JStech for implementing the changes! Thanks @felixvd @tandronescu @davetcoleman for your comments and keep tracking on the progress!

RoboticsYY · 2020-08-14T06:08:21Z

Add un-resolved comments to issues: #16 #17 #18 #19 #20.

RoboticsYY commented Jun 6, 2020

View reviewed changes

RoboticsYY mentioned this pull request Jun 6, 2020

Handeye calibration rviz gui plugin [WIP] moveit/moveit#1560

Closed

4 tasks

RoboticsYY force-pushed the pr-add_handeye_calibration_rviz_plugin branch from 44737a5 to 7211cc2 Compare June 6, 2020 14:10

RoboticsYY changed the title ~~Pr-add handeye calibration rviz plugin~~ [WIP] Pr-add handeye calibration rviz plugin Jun 6, 2020

RoboticsYY force-pushed the pr-add_handeye_calibration_rviz_plugin branch from 39eb2f6 to f9b4f55 Compare June 6, 2020 16:06

JStech reviewed Jun 11, 2020

View reviewed changes

RoboticsYY mentioned this pull request Jun 16, 2020

Extending MoveIt with hand-eye calibration moveit/moveit#1070

Closed

JStech reviewed Jun 22, 2020

View reviewed changes

RoboticsYY mentioned this pull request Jun 23, 2020

Plugin library is not found after building #9

Open

RoboticsYY and others added 9 commits June 24, 2020 12:08

Add .gitignore

0971343

Add moveit_calibration_gui package.xml and CMakeLists.txt files

f673c70

Add handeye calibration rviz plugin export files

fcaa1e1

Add handeye calibration rviz panel

fefb29d

Add handeye calibration target detection widget

cdba876

Add handeye calibration context configuration widget

4b4a11c

Add handeye calibration control operation widget

5ddf1fa

Enable this repo build with moveit in CI

a7c4736

some suggested changes to GUI text

c698077

RoboticsYY force-pushed the pr-add_handeye_calibration_rviz_plugin branch from b05ce3c to c698077 Compare June 24, 2020 04:08

tandronescu reviewed Jun 30, 2020

View reviewed changes

John Stechschulte added 2 commits July 20, 2020 22:20

Merged rviz pr into charuco-board

bfc5fe2

updated gui to work with changing parameters

4934340

John Stechschulte added 9 commits July 20, 2020 22:20

not sure--lots of repeats from charuco-board branch

55ca5be

added rviz_visual_tools to CMakeLists and package.xml

80c4266

saving and loading config works

774b4af

check for OpenCV 3.(2+)

3109a10

added default values to target parameters

3c2fc4c

load default target parameter values in gui

127c1de

load target parameter input widgets when panel is created

df1bd19

actual fix to OpenCV version issue

9998f36

trigger sensor mount update when loading panel

4a86db6

JStech approved these changes Jul 28, 2020

View reviewed changes

JStech mentioned this pull request Aug 5, 2020

Review comments RoboticsYY/moveit_calibration#3

Merged

John Stechschulte added 6 commits August 13, 2020 16:31

reload saved solver option

2c3a18c

more comments from Felix

dac9c5e

more comments

c256ca6

auto calibration interface changes

04a95e1

more descriptive error messages

124cc07

not sure why this wasn't generating an error

8ff0ee7

JStech mentioned this pull request Aug 13, 2020

Separate out functionality in calibration tab #15

Open

JStech approved these changes Aug 13, 2020

View reviewed changes

RoboticsYY merged commit b7d1921 into moveit:master Aug 14, 2020

RoboticsYY changed the title ~~[WIP] Pr-add handeye calibration rviz plugin~~ Pr-add handeye calibration rviz plugin Aug 14, 2020

prasuchit mentioned this pull request Jul 19, 2022

Rviz crashing while trying to create target #110

Closed

	std::map<std::string, std::string> frame_names_;
	std::map<std::string, std::string> frame_names_ = "base";


		std::ostringstream ss;

		QStandardItem* child_1 = new QStandardItem("bTe");

Pr-add handeye calibration rviz plugin #4

Pr-add handeye calibration rviz plugin #4

Conversation

RoboticsYY commented Jun 6, 2020

RoboticsYY commented Jun 6, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

JStech commented Jun 8, 2020 • edited Loading

RoboticsYY commented Jun 8, 2020

JStech commented Jun 10, 2020 • edited Loading

Choose a reason for hiding this comment

RoboticsYY Jun 11, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

RoboticsYY commented Jun 11, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tandronescu commented Jun 30, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tandronescu commented Jul 8, 2020

JStech left a comment

Choose a reason for hiding this comment

RoboticsYY commented Jul 29, 2020

JStech commented Aug 5, 2020

davetcoleman commented Aug 12, 2020

tandronescu commented Aug 12, 2020

JStech left a comment • edited Loading

Choose a reason for hiding this comment

RoboticsYY commented Aug 14, 2020

RoboticsYY commented Aug 14, 2020

JStech commented Jun 8, 2020 •

edited

Loading

JStech commented Jun 10, 2020 •

edited

Loading

RoboticsYY Jun 11, 2020 •

edited

Loading

RoboticsYY commented Jun 11, 2020 •

edited

Loading

JStech left a comment •

edited

Loading