Git Product home page Git Product logo

ros2_grasp_library's Introduction

DISCONTINUATION OF PROJECT

This project will no longer be maintained by Intel.
Intel has ceased development and contributions including, but not limited to, maintenance, bug fixes, new releases, or updates, to this project.
Intel no longer accepts patches to this project.
If you have an ongoing need to use this project, are interested in independently developing it, or would like to maintain patches for the open source software community, please create your own fork of this project.

Contact: [email protected]

ROS2 Grasp Library

A ROS2 intelligent visual grasp solution for advanced industrial usages, with OpenVINO™ grasp detection and MoveIt Grasp Planning.

Overview

ROS2 Grasp Library enables state-of-the-art CNN based deep learning grasp detection algorithms on ROS2 for intelligent visual grasp in industrial robot usage scenarios. This package provides ROS2 interfaces compliant with the open source MoveIt motion planning framework supported by most of the robot models in ROS industrial. This package delivers

  • A ROS2 Grasp Planner providing grasp planning service, as an extensible capability of MoveIt (moveit_msgs::srv::GraspPlanning), translating grasp detection results into MoveIt Interfaces (moveit_msgs::msg::Grasp)
  • A ROS2 Grasp Detctor abstracting interfaces for grasp detection results
  • A ROS2 hand-eye calibration module generating transformation from camera frame to robot frame
  • ROS2 example applications demonstrating how to use this ROS2 Grasp Library in advanced industrial usages for intelligent visual grasp

Grasp Detection Algorithms

Grasp detection back-end algorithms enabled by this ROS2 Grasp Library:

  • Grasp Pose Detection detects 6-DOF grasp poses for a 2-finger grasp (e.g. a parallel jaw gripper) in 3D point clouds from RGBD sensor or PCD file. The grasp detection was enabled with Intel® DLDT toolkit and Intel® OpenVINO™ toolkit.

    ROS2 Grasp Library

Tutorials

Refer to ROS2 Grasp Library Tutorials for how to

  • Install, build, and launch the ROS2 Grasp Planner and Detector
  • Use launch options to customize in a new workspace
  • Bring up the intelligent visual grasp solution on a new robot
  • Do hand-eye calibration for a new camera setup
  • Launch the example applications

Example Applications

Random Picking (OpenVINO Grasp Detection)

Random Pick with OpenVINO Grasp Detection - Link to Youtube video demo

Recognition Picking (OpenVINO Grasp Detection + OpenVINO Mask-rcnn Object Segmentation)

Recognition Pick with OpenVINO Grasp Detection - Link to Youtube video demo

Known Issues

  • Cloud camera failed at "Invalid sizes when resizing a matrix or array" when dealing with XYZRGBA pointcloud from ROS2 Realsenes, tracked as #6 of gpg, patch under review.
  • 'colcon test' sometimes failed with test suite "tgrasp_ros2", due to ROS2 service request failure issue (reported ros2 examples issue #228 and detailed discussed in ros2 demo issue #304)
  • Rviz2 failed to receive Static TF from camera due to transient_local QoS (expected in the coming ROS2 Eloquent, discussed in geometry2 issue #183), workaround patch available till the adaption to Eloquent

Contribute to This Project

It's welcomed to contribute to this project. Here're some recommended practices:

  • When adding a new feature it's expected to add tests covering the new functionalities
    colcon test --packages-select <names_of_affected_packages>
  • Before submitting a patch, it's recommended to pass all existing tests to avoid regression
    colcon test --packages-select <names_of_existing_packages>
Any security issue should be reported using process at https://01.org/security

ros2_grasp_library's People

Contributors

congliu0913 avatar rafaelhuang-intel avatar rdower avatar roboticsyy avatar sfblackl-intel avatar sharronliu avatar yechun1 avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

ros2_grasp_library's Issues

Segmentation Fault when used with realsense-ros eloquent branch

I am trying to use this library with the L515 LiDar camera from intel. Since the https://github.com/intel/ros2_intel_realsense repository doesn't have support for that camera I attempted to use the eloquent branch of https://github.com/IntelRealSense/realsense-ros.
This throws a Segmentation fault error when the /plan_grasps service is called and crashes the ROS package. I'm suspecting it may be due to a difference in the structure of PointCloud2 messages being delivered.

Please help

[ERROR] colcon build --symlink-install --packages-select grasp_msgs moveit_msgs grasp_ros2

When building the grasp library, this step. An error occurred:

colcon build --symlink-install --packages-select grasp_msgs moveit_msgs grasp_ros2
Starting >>> grasp_msgs
Starting >>> moveit_msgs
Finished <<< grasp_msgs [0.64s]
Finished <<< moveit_msgs [0.80s]
Starting >>> grasp_ros2
--- stderr: grasp_ros2
CMake Error at CMakeLists.txt:36 (find_package):
By not providing "Findament_cmake.cmake" in CMAKE_MODULE_PATH this project
has asked CMake to find a package configuration file provided by
"ament_cmake", but CMake did not find one.

Could not find a package configuration file provided by "ament_cmake" with
any of the following names:

ament_cmakeConfig.cmake
ament_cmake-config.cmake

Add the installation prefix of "ament_cmake" to CMAKE_PREFIX_PATH or set
"ament_cmake_DIR" to a directory containing one of the above files. If
"ament_cmake" provides a separate development package or SDK, be sure it
has been installed.


Failed <<< grasp_ros2 [0.08s, exited with code 1]

Summary: 2 packages finished [1.29s]
1 package failed: grasp_ros2
1 package had stderr output: grasp_ros2

Does someone know how to fix this????
Thanks a lot!

PoseEstimator::imageCB_ARUCO does not use refined corner

The PoseEstimator::imageCB_ARUCO uses cv::aruco::refineDetectedMarkers to refine detected corners. But, this method does not use refined corner.

std::vector<std::vector<cv::Point2f>> corners;
cv::aruco::detectMarkers(cv_ptr->image, dictionary, corners, ids);

if (ids.size() > 0)
{
  cv::Mat imageColor;
  cv::cvtColor(cv_ptr->image, imageColor, cv::COLOR_GRAY2RGB);
  std::vector<std::vector<cv::Point2f>> rejectedCorners;
  cv::aruco::refineDetectedMarkers(cv_ptr->image, board, corners, ids, rejectedCorners, camera_matrix_,
                                   dist_coeffs_);
  cv::aruco::drawDetectedMarkers(imageColor, corners, ids);

  // Estimate the pose of aruco board
  cv::Vec3d rvect, tvect;
  int valid = cv::aruco::estimatePoseBoard(corners, ids, board, camera_matrix_, dist_coeffs_, rvect, tvect);

Error when building

Hello
Thank you for your contribution

When compiling ros2_grasp_library, I occurred the following error

[32.086s] WARNING:colcon.colcon_cmake.task.cmake.build:Could not build CMake package 'recognize_pick' because the CMake cache has no 'CMAKE_PROJECT_NAME' variable
--- stderr: recognize_pick
Traceback (most recent call last):
  File "/usr/lib/python3/dist-packages/colcon_core/executor/__init__.py", line 91, in __call__
    rc = await self.task(*args, **kwargs)
  File "/usr/lib/python3/dist-packages/colcon_core/task/__init__.py", line 93, in __call__
    return await task_method(*args, **kwargs)
  File "/usr/lib/python3/dist-packages/colcon_ros/task/ament_cmake/build.py", line 65, in build
    has_install_target = await has_target(args.build_base, 'install')
  File "/usr/lib/python3/dist-packages/colcon_cmake/task/cmake/__init__.py", line 57, in has_target
    return target in await get_makefile_targets(path)
  File "/usr/lib/python3/dist-packages/colcon_cmake/task/cmake/__init__.py", line 78, in get_makefile_targets
    CMAKE_EXECUTABLE, '--build', path, '--target', 'help'], cwd=path)
  File "/usr/lib/python3/dist-packages/colcon_core/subprocess.py", line 110, in check_output
    'Expected {args} to pass: {stderr_data}'.format_map(locals())
AssertionError: Expected ['/usr/local/bin/cmake', '--build', '/datadrive/workspace/ros2_ws/build/recognize_pick', '--target', 'help'] to pass: Error: could not find CMAKE_PROJECT_NAME in Cache

---
Failed   <<< recognize_pick [0.11s, exited with code 1]
Aborted  <<< random_pick [1.56s]                                                                                                                                  
Aborted  <<< handeye_tf_service [17.9s]                                                                                                                            
Aborted  <<< draw_x [9.56s]                                                                                                                                         
Aborted  <<< fixed_position_pick [9.55s]
Aborted  <<< grasp_ros2 [8.69s]                                                                                        
Aborted  <<< handeye_target_detection [22.9s]                                    
                                   
Summary: 20 packages finished [40.8s]
  1 package failed: recognize_pick
  6 packages aborted: draw_x fixed_position_pick grasp_ros2 handeye_target_detection handeye_tf_service random_pick
  3 packages had stderr output: grasp_ros2 handeye_target_detection recognize_pick
Expected ['/usr/local/bin/cmake', '--build', '/datadrive/workspace/ros2_ws/build/recognize_pick', '--target', 'help'] to pass: Error: could not find CMAKE_PROJECT_NAME in Cache

Do you guys know how to fix this?
Thanks

Error when running random_pick.yaml

Environment: Ubuntu 18.04+ROS2 dashing+UR5

I modified some data in random_pick.yaml according to our lab's setup after finishing handeye calibration for attached camera amounted on the end effector. However, it unexpectedly went wrong when running random_pick.yaml showing the error as follows:

ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/random_pick.yaml

terminate called after throwing an instance of 'std::runtime_error'
what(): Failed to parse parameters from file 'src/ros2_grasp_library/grasp_ros2/cfg/random_pick.yaml': Error parsing a event near line 10, at /tmp/binarydeb/ros-dashing-rcl-yaml-param-parser-0.7.9/src/parser.c:1213

I'd appreciate it a lot if anyone can give any suggestions. Thank you so much!

Eigen 3.2 is required by GPD

Recently we encountered random segmentation fault when deploy the software on several new machines. After investigation the problem is root caused to the Eigen (libeigen3-dev) version 3.3 installed from Ubuntu 18.04 bionic.
We also found the problem disappeared when replace Eigen with version 3.2
We shall update the docker scripts to use Eigen version 3.2. And make sure components dependent on Eigen, like PCL, GPD, and Grasp Library, are built after that.

Details

  • stdout log
    Estimating local reference frames ...
    Estimated 3 frames in 0.0000s.
    Finding hand poses ...
    Segmentation fault (core dumped)

  • gdb backtrace
    (gdb) bt
    #0 0x00007ffa5582898d in __GI___libc_free (mem=0x25) at malloc.c:3103
    #1 0x00007ffa5d808c0f in Eigen::internal::handmade_aligned_free(void*) (ptr=) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:98
    #2 0x00007ffa5d808c0f in Eigen::internal::aligned_free(void*) (ptr=) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:179
    #3 0x00007ffa5d808c0f in Eigen::internal::conditional_aligned_free(void*) (ptr=)
    at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:230
    #4 0x00007ffa5d808c0f in Eigen::internal::conditional_aligned_delete_auto<double, true>(double*, unsigned long) (size=, ptr=) at /usr/include/eigen3/Eigen/src/Core/util/Memory.h:416
    #5 0x00007ffa5d808c0f in Eigen::DenseStorage<double, -1, 3, -1, 0>::~DenseStorage() (this=0x7ffa2a2c5668, __in_chrg=)
    at /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:468
    #6 0x00007ffa5d808c0f in Eigen::PlainObjectBase<Eigen::Matrix<double, 3, -1, 0, 3, -1> >::~PlainObjectBase() (this=0x7ffa2a2c5668, __in_chrg=) at /usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:98
    #7 0x00007ffa5d808c0f in Eigen::Matrix<double, 3, -1, 0, 3, -1>::~Matrix() (this=0x7ffa2a2c5668, __in_chrg=)
    at /usr/include/eigen3/Eigen/src/Core/Matrix.h:178
    #8 0x00007ffa5d808c0f in gpd::util::PointList::~PointList() (this=0x7ffa2a2c5630, __in_chrg=)
    at /home/intel/workspace/views/gpd/include/gpd/util/point_list.h:57
    #9 0x00007ffa5d808c0f in _ZNK3gpd9candidate10HandSearch9evalHandsERKNS_4util5CloudERKSt6vectorINS0_10LocalFrameESaIS7_EERKN3pcl11KdTreeFLANNINSC_12PointXYZRGBAEN5flann9L2_SimpleIfEEEE._omp_fn.1(void) () at /home/intel/workspace/views/gpd/src/gpd/candidate/hand_search.cpp:169
    #10 0x00007ffa55db096e in () at /usr/lib/x86_64-linux-gnu/libgomp.so.1
    #11 0x00007ffa590936db in start_thread (arg=0x7ffa2a2c6700) at pthread_create.c:463

ROS2 Humble Support

Hi, I am using ROS2 Humble (Ubuntu 22) to compile this grasp library.
However, there are errors related to compiling GPD library. Are there any updates on this?
Thanks!

Eigen Error

Due to the Nvidia GPU is not supported? I set device to CPU in grasp_ros2_params.yaml . and run ros2 run grasp_ros2 grasp_ros2 __params:=src/ros2_grasp_library/grasp_ros2/cfg/grasp_ros2_params.yaml

met with the problem following:

Calculating surface normals ...
Using integral images for surface normals estimation ...
grasp_ros2: /usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h:285: void Eigen::PlainObjectBase<Derived>::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Matrix<double, 3, -1>; Eigen::Index = long int]: Assertion `(!(RowsAtCompileTime!=Dynamic) || (rows==RowsAtCompileTime)) && (!(ColsAtCompileTime!=Dynamic) || (cols==ColsAtCompileTime)) && (!(RowsAtCompileTime==Dynamic && MaxRowsAtCompileTime!=Dynamic) || (rows<=MaxRowsAtCompileTime)) && (!(ColsAtCompileTime==Dynamic && MaxColsAtCompileTime!=Dynamic) || (cols<=MaxColsAtCompileTime)) && rows>=0 && cols>=0 && "Invalid sizes when resizing a matrix or array."' failed.

my assumption:
Because of the object(the scene) in realsense view could not be calculated currently, or due to the gpd model?

Poor alignment of the axes on the templates

I am trying to perform a handeye calibration with a 6 DOF robotic arm using MoveIt. I am using the utilities of the ros2_grasp_library package, specifically the handeye_dashboard. So far, I have not been able to achieve a successful calibration. I have noticed that the XYZ axis positions in the handeye_target_detection tool do not match the documentation.
Screenshot from 2023-04-25 11-04-56
Screenshot from 2023-04-25 12-26-13

Error when run colcon build

Hello!
I faced with error when i try run this command:
colcon build --symlink-install --packages-select grasp_msgs moveit_msgs grasp_ros2

colcon build --symlink-install --packages-select grasp_msgs moveit_msgs grasp_ros2
Starting >>> grasp_msgs
Starting >>> moveit_msgs
Finished <<< grasp_msgs [1.38s]                                                                 
Finished <<< moveit_msgs [1.65s]                  
Starting >>> grasp_ros2
[Processing: grasp_ros2]                             
[Processing: grasp_ros2]                                     
--- stderr: grasp_ros2                                         
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_planner.cpp:29:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:5: error: ‘GraspDetector’ has not been declared
     GraspDetector::GraspDetectionParameters & param);
     ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:45: error: expected ‘,’ or ‘...’ before ‘&’ token
     GraspDetector::GraspDetectionParameters & param);
                                             ^
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:18:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:5: error: ‘GraspDetector’ has not been declared
     GraspDetector::GraspDetectionParameters & param);
     ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:45: error: expected ‘,’ or ‘...’ before ‘&’ token
     GraspDetector::GraspDetectionParameters & param);
                                             ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:3: error: ‘GraspDetector’ has not been declared
   GraspDetector::GraspDetectionParameters & param)
   ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:43: error: expected ‘,’ or ‘...’ before ‘&’ token
   GraspDetector::GraspDetectionParameters & param)
                                           ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp: In static member function ‘static void grasp_ros2::ROSParameters::getDetectionParams(rclcpp::Node*, int)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:28:42: error: ‘param’ was not declared in this scope
   node->get_parameter_or("finger_width", param.hand_search_params.finger_width_, 0.005);
                                          ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:18: warning: unused parameter ‘GraspDetectionParameters’ [-Wunused-parameter]
   GraspDetector::GraspDetectionParameters & param)
                  ^~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:18:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:5: error: ‘GraspDetector’ has not been declared
     GraspDetector::GraspDetectionParameters & param);
     ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:45: error: expected ‘,’ or ‘...’ before ‘&’ token
     GraspDetector::GraspDetectionParameters & param);
                                             ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:3: error: ‘GraspDetector’ has not been declared
   GraspDetector::GraspDetectionParameters & param)
   ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:43: error: expected ‘,’ or ‘...’ before ‘&’ token
   GraspDetector::GraspDetectionParameters & param)
                                           ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp: In static member function ‘static void grasp_ros2::ROSParameters::getDetectionParams(rclcpp::Node*, int)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:28:42: error: ‘param’ was not declared in this scope
   node->get_parameter_or("finger_width", param.hand_search_params.finger_width_, 0.005);
                                          ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/ros_params.cpp:25:18: warning: unused parameter ‘GraspDetectionParameters’ [-Wunused-parameter]
   GraspDetector::GraspDetectionParameters & param)
                  ^~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:98:15: error: ‘Grasp’ was not declared in this scope
   std::vector<Grasp> detectGraspPosesInTopic();
               ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:98:15: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:98:20: error: template argument 1 is invalid
   std::vector<Grasp> detectGraspPosesInTopic();
                    ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:98:20: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:117:73: error: ‘Grasp’ was not declared in this scope
   grasp_msgs::msg::GraspConfigList createGraspListMsg(const std::vector<Grasp> & hands);
                                                                         ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:117:73: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:117:78: error: template argument 1 is invalid
   grasp_msgs::msg::GraspConfigList createGraspListMsg(const std::vector<Grasp> & hands);
                                                                              ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:117:78: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:124:56: error: ‘Grasp’ does not name a type
   grasp_msgs::msg::GraspConfig convertToGraspMsg(const Grasp & hand);
                                                        ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:130:23: error: ‘Grasp’ was not declared in this scope
     const std::vector<Grasp> & hands,
                       ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:130:23: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:130:28: error: template argument 1 is invalid
     const std::vector<Grasp> & hands,
                            ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:130:28: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:197:19: error: ‘GraspDetector’ was not declared in this scope
   std::shared_ptr<GraspDetector> grasp_detector_; /**< used to run the grasp pose detection*/
                   ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:197:19: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:41:0,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/usr/local/include/gpd/grasp_detector.h:66:7: note:   ‘gpd::GraspDetector’
 class GraspDetector {
       ^~~~~~~~~~~~~
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:197:32: error: template argument 1 is invalid
   std::shared_ptr<GraspDetector> grasp_detector_; /**< used to run the grasp pose detection*/
                                ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:198:3: error: ‘GraspDetector’ does not name a type; did you mean ‘GraspDetectorGPD’?
   GraspDetector::GraspDetectionParameters detection_param_; /**< grasp detector parameters*/
   ^~~~~~~~~~~~~
   GraspDetectorGPD
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:25:0:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:5: error: ‘GraspDetector’ has not been declared
     GraspDetector::GraspDetectionParameters & param);
     ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/ros_params.hpp:38:45: error: expected ‘,’ or ‘...’ before ‘&’ token
     GraspDetector::GraspDetectionParameters & param);
                                             ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In constructor ‘grasp_ros2::GraspDetectorGPD::GraspDetectorGPD(const rclcpp::NodeOptions&)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:89:43: error: ‘detection_param_’ was not declared in this scope
   ROSParameters::getDetectionParams(this, detection_param_);
                                           ^~~~~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:89:43: note: suggested alternative: ‘detector_thread_’
   ROSParameters::getDetectionParams(this, detection_param_);
                                           ^~~~~~~~~~~~~~~~
                                           detector_thread_
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:90:38: error: ‘GraspDetector’ was not declared in this scope
   grasp_detector_ = std::make_shared<GraspDetector>(detection_param_);
                                      ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:90:38: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:41:0,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/usr/local/include/gpd/grasp_detector.h:66:7: note:   ‘gpd::GraspDetector’
 class GraspDetector {
       ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘void grasp_ros2::GraspDetectorGPD::onInit()’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:105:19: error: ‘Grasp’ was not declared in this scope
       std::vector<Grasp> grasps = detectGraspPosesInTopic();
                   ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:105:19: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:105:24: error: template argument 1 is invalid
       std::vector<Grasp> grasps = detectGraspPosesInTopic();
                        ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:105:24: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:108:15: error: ‘HandSearch’ does not name a type; did you mean ‘cvSeqSearch’?
         const HandSearch::Parameters & params = grasp_detector_->getHandSearchParameters();
               ^~~~~~~~~~
               cvSeqSearch
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:109:67: error: ‘params’ was not declared in this scope
         grasps_rviz_pub_->publish(convertToVisualGraspMsg(grasps, params.hand_outer_diameter_,
                                                                   ^~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: At global scope:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:124:13: error: ‘Grasp’ was not declared in this scope
 std::vector<Grasp> GraspDetectorGPD::detectGraspPosesInTopic()
             ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:124:13: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:124:18: error: template argument 1 is invalid
 std::vector<Grasp> GraspDetectorGPD::detectGraspPosesInTopic()
                  ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:124:18: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘int grasp_ros2::GraspDetectorGPD::detectGraspPosesInTopic()’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:127:15: error: ‘Grasp’ was not declared in this scope
   std::vector<Grasp> grasps;
               ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:127:15: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:127:20: error: template argument 1 is invalid
   std::vector<Grasp> grasps;
                    ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:127:20: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:131:20: error: base operand of ‘->’ is not a pointer
     grasp_detector_->preprocessPointCloud(*cloud_camera_);
                    ^~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:133:29: error: base operand of ‘->’ is not a pointer
     grasps = grasp_detector_->detectGrasps(*cloud_camera_);
                             ^~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘void grasp_ros2::GraspDetectorGPD::cloud_callback(sensor_msgs::msg::PointCloud2_<std::allocator<void> >::SharedPtr)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:187:34: error: ‘detection_param_’ was not declared in this scope
         if (cloud->points[i].x > detection_param_.workspace_[0] && cloud->points[i].x < detection_param_.workspace_[1] &&
                                  ^~~~~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:187:34: note: suggested alternative: ‘detector_thread_’
         if (cloud->points[i].x > detection_param_.workspace_[0] && cloud->points[i].x < detection_param_.workspace_[1] &&
                                  ^~~~~~~~~~~~~~~~
                                  detector_thread_
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: At global scope:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:299:21: error: ‘Grasp’ was not declared in this scope
   const std::vector<Grasp> & hands)
                     ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:299:21: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:299:26: error: template argument 1 is invalid
   const std::vector<Grasp> & hands)
                          ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:299:26: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘grasp_msgs::msg::GraspConfigList grasp_ros2::GraspDetectorGPD::createGraspListMsg(const int&)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:303:34: error: request for member ‘size’ in ‘hands’, which is of non-class type ‘const int’
   for (uint32_t i = 0; i < hands.size(); i++) {
                                  ^~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:304:51: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     msg.grasps.push_back(convertToGraspMsg(hands[i]));
                                                   ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: At global scope:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:313:72: error: ‘Grasp’ does not name a type
 grasp_msgs::msg::GraspConfig GraspDetectorGPD::convertToGraspMsg(const Grasp & hand)
                                                                        ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘grasp_msgs::msg::GraspConfig grasp_ros2::GraspDetectorGPD::convertToGraspMsg(const int&)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:316:24: error: request for member ‘getGraspBottom’ in ‘hand’, which is of non-class type ‘const int’
   pointEigenToMsg(hand.getGraspBottom(), msg.bottom);
                        ^~~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:317:24: error: request for member ‘getGraspTop’ in ‘hand’, which is of non-class type ‘const int’
   pointEigenToMsg(hand.getGraspTop(), msg.top);
                        ^~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:318:24: error: request for member ‘getGraspSurface’ in ‘hand’, which is of non-class type ‘const int’
   pointEigenToMsg(hand.getGraspSurface(), msg.surface);
                        ^~~~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:319:25: error: request for member ‘getApproach’ in ‘hand’, which is of non-class type ‘const int’
   vectorEigenToMsg(hand.getApproach(), msg.approach);
                         ^~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:320:25: error: request for member ‘getBinormal’ in ‘hand’, which is of non-class type ‘const int’
   vectorEigenToMsg(hand.getBinormal(), msg.binormal);
                         ^~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:321:25: error: request for member ‘getAxis’ in ‘hand’, which is of non-class type ‘const int’
   vectorEigenToMsg(hand.getAxis(), msg.axis);
                         ^~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:322:25: error: request for member ‘getGraspWidth’ in ‘hand’, which is of non-class type ‘const int’
   msg.width.data = hand.getGraspWidth();
                         ^~~~~~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:323:25: error: request for member ‘getScore’ in ‘hand’, which is of non-class type ‘const int’
   msg.score.data = hand.getScore();
                         ^~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:324:24: error: request for member ‘getSample’ in ‘hand’, which is of non-class type ‘const int’
   pointEigenToMsg(hand.getSample(), msg.sample);
                        ^~~~~~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: At global scope:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:330:21: error: ‘Grasp’ was not declared in this scope
   const std::vector<Grasp> & hands,
                     ^~~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:330:21: note: suggested alternative:
In file included from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning__struct.hpp:32:0,
                 from /home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/srv/grasp_planning.hpp:7,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_planner.hpp:22,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/include/grasp_library/ros2/grasp_detector_gpd.hpp:54,
                 from /home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:24:
/home/tucuman/ros2_ws/install/moveit_msgs/include/moveit_msgs/msg/grasp__struct.hpp:267:49: note:   ‘moveit_msgs::msg::Grasp’
   moveit_msgs::msg::Grasp_<std::allocator<void>>;
                                                 ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:330:26: error: template argument 1 is invalid
   const std::vector<Grasp> & hands,
                          ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:330:26: error: template argument 2 is invalid
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp: In member function ‘visualization_msgs::msg::MarkerArray grasp_ros2::GraspDetectorGPD::convertToVisualGraspMsg(const int&, double, double, double, double, const string&)’:
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:343:34: error: request for member ‘size’ in ‘hands’, which is of non-class type ‘const int’
   for (uint32_t i = 0; i < hands.size(); i++) {
                                  ^~~~
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:344:26: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     left_bottom = hands[i].getGraspBottom() - (hw - 0.5 * finger_width) * hands[i].getBinormal();
                          ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:344:82: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     left_bottom = hands[i].getGraspBottom() - (hw - 0.5 * finger_width) * hands[i].getBinormal();
                                                                                  ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:345:27: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     right_bottom = hands[i].getGraspBottom() + (hw - 0.5 * finger_width) * hands[i].getBinormal();
                           ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:345:83: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     right_bottom = hands[i].getGraspBottom() + (hw - 0.5 * finger_width) * hands[i].getBinormal();
                                                                                   ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:346:50: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     left_top = left_bottom + hand_depth * hands[i].getApproach();
                                                  ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:347:52: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     right_top = right_bottom + hand_depth * hands[i].getApproach();
                                                    ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:350:84: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     base_center = left_bottom + 0.5 * (right_bottom - left_bottom) - 0.01 * hands[i].getApproach();
                                                                                    ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:351:51: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
     approach_center = base_center - 0.04 * hands[i].getApproach();
                                                   ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:354:16: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
         hands[i].getFrame(), 0.02, hand_height, i, frame_id);
                ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:356:16: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
         hands[i].getFrame(), hand_depth, finger_width, hand_height, i * 3, frame_id);
                ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:358:16: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
         hands[i].getFrame(), hand_depth, finger_width, hand_height, i * 3 + 1, frame_id);
                ^
/home/tucuman/ros2_ws/src/ros2_grasp_library/grasp_ros2/src/grasp_detector_gpd.cpp:360:16: error: invalid types ‘const int[uint32_t {aka unsigned int}]’ for array subscript
         hands[i].getFrame(), 0.08, finger_width, hand_height, i * 3 + 2, frame_id);
                ^
c++: internal compiler error: Killed (program cc1plus)
Please submit a full bug report,
with preprocessed source if appropriate.
See <file:///usr/share/doc/gcc-7/README.Bugs> for instructions.
make[2]: *** [CMakeFiles/grasp_plan.dir/src/grasp_planner.cpp.o] Error 4
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/grasp_detect.dir/src/ros_params.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/grasp_plan.dir/src/ros_params.cpp.o] Error 1
make[1]: *** [CMakeFiles/grasp_plan.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
make[2]: *** [CMakeFiles/grasp_detect.dir/src/grasp_detector_gpd.cpp.o] Error 1
make[1]: *** [CMakeFiles/grasp_detect.dir/all] Error 2
make: *** [all] Error 2
---
Failed   <<< grasp_ros2 [1min 16s, exited with code 2]
                                      
Summary: 2 packages finished [1min 18s]
  1 package failed: grasp_ros2
  1 package had stderr output: grasp_ros2

How can i fix this?
Many thanks!

Note: any robot can be used, only ensure that the robot ROS2 driver is publishing the joint states and link TFs at rate of at least 125Hz

Hi,I'm using hans elfin5 robot instead of ur5.I don't know how to link Han's robotic arm drive with ros2_grasp_library,Should Terminal 1 and Terminal 2 be running?

Terminal 1 (robot frames tf update)

ros2 launch robot_interface ur_test.launch.py move:=false

Terminal 2 (robot state display in Rviz2)

ros2 launch ur_description view_ur5_ros2.launch.py

Looking forward to your reply, thank you.

TF2 Linking error

Upon building this package, I get this linking error (See below). I'm not very well versed in CMake, so I'd greatly appreciate any help in debugging this!

/usr/bin/ld: CMakeFiles/pose_estimation.dir/src/pose_estimator.cpp.o: in function `PoseEstimator::imageCB_CHESSBOARD(std::shared_ptr<sensor_msgs::msg::Image_<std::allocator<void> > const> const&)':
pose_estimator.cpp:(.text+0x243c): undefined reference to `tf2::transformTF2ToMsg(tf2::Transform const&, geometry_msgs::msg::TransformStamped_<std::allocator<void> >&, builtin_interfaces::msg::Time_<std::allocator<void> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
/usr/bin/ld: CMakeFiles/pose_estimation.dir/src/pose_estimator.cpp.o: in function `PoseEstimator::imageCB_ASYMMETRIC_CIRCLES_GRID(std::shared_ptr<sensor_msgs::msg::Image_<std::allocator<void> > const> const&)':
pose_estimator.cpp:(.text+0x3ae1): undefined reference to `tf2::transformTF2ToMsg(tf2::Transform const&, geometry_msgs::msg::TransformStamped_<std::allocator<void> >&, builtin_interfaces::msg::Time_<std::allocator<void> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
/usr/bin/ld: CMakeFiles/pose_estimation.dir/src/pose_estimator.cpp.o: in function `PoseEstimator::imageCB_ARUCO(std::shared_ptr<sensor_msgs::msg::Image_<std::allocator<void> > const> const&)':
pose_estimator.cpp:(.text+0x5338): undefined reference to `tf2::transformTF2ToMsg(tf2::Transform const&, geometry_msgs::msg::TransformStamped_<std::allocator<void> >&, builtin_interfaces::msg::Time_<std::allocator<void> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
/usr/bin/ld: CMakeFiles/pose_estimation.dir/src/pose_estimator.cpp.o: in function `PoseEstimator::imageCB_CHARUCO(std::shared_ptr<sensor_msgs::msg::Image_<std::allocator<void> > const> const&)':
pose_estimator.cpp:(.text+0x66b5): undefined reference to `tf2::transformTF2ToMsg(tf2::Transform const&, geometry_msgs::msg::TransformStamped_<std::allocator<void> >&, builtin_interfaces::msg::Time_<std::allocator<void> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&)'
collect2: error: ld returned 1 exit status

handeye_target_detection use case

colcon build error:

ros2_ws/src/ros2_grasp_library/grasp_utils/handeye_target_detection/src/pose_estimator.cpp In member function ‘void PoseEstimator::imageCB_CHARUCO(const ConstSharedPtr&)’:
/home/lab001/ros2_ws/src/ros2_grasp_library/grasp_utils/handeye_target_detection/src/pose_estimator.cpp:353:19: error: ‘struct cv::aruco::DetectorParameters’ has no member named ‘cornerRefinementMethod’; did you mean ‘cornerRefinementWinSize’?
       params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
                   ^~~~~~~~~~~~~~~~~~~~~~
                   cornerRefinementWinSize
/home/lab001/ros2_ws/src/ros2_grasp_library/grasp_utils/handeye_target_detection/src/pose_estimator.cpp:353:55: error: ‘CORNER_REFINE_SUBPIX’ is not a member of ‘cv::aruco’
       params_ptr->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
                                                       ^~~~~~~~~~~~~~~~~~~~
make[2]: *** [CMakeFiles/pose_estimation.dir/src/pose_estimator.cpp.o] Error 1

error

cause by opencv, I am not sure.

Transformation from end-effector to camera_link

Hi.
As I understand, the goal of the hand-in-eye calibration process is to achieve a static transformation between the end-effector and the camera_link, which is static when the robot moves.
In the tutorial, the result is a static transform between the base and the camera. Is this the result when the camera is attached to the manipulator? Is there any way to retrieve the static TF between the end-effector and the camera_link with the tools?

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.