Constraint samplers #60

vmayoral · 2019-03-30T22:34:33Z

Unit tests (pending on addressing Unit tests in moveit2 failing AcutronicRobotics/moveit2#13)

henningkayser

Looks good in general, just some minor changes.

henningkayser · 2019-04-11T17:13:54Z

moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp

@@ -61,16 +63,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
  if (!jmg)
    return constraint_samplers::ConstraintSamplerPtr();
  std::stringstream ss;
-  ss << constr;
-  ROS_DEBUG_NAMED("constraint_samplers",
+  ss << constr.name;


Isn't there a way to print the whole message as a string anymore?

I still think that the full constraint should be printed here instead of only the name.

There is no such implementation for ROS2. I have asked that on the following issue : ros2/common_interfaces#69 (comment) redirected to ros2/rosidl#259

henningkayser · 2019-04-11T17:22:28Z

moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp

-  ros::WallTime end = ros::WallTime::now() + ros::WallDuration(1.0);
+  //TODO RCLCPP::WallTimer has not a .now() function, and there is no wallDuration (they use std::chrono::nanoseconds)
+  // I'll use rclcpp::time, but this is something to fix.
+  rclcpp::Duration duration(1,0);


I think this solution is good. We could instantiate rclcpp::Duration inline, though.

henningkayser · 2019-04-11T17:23:48Z

moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp

-    visualization_msgs::Marker mk;
-    mk.header.stamp = ros::Time::now();
+    visualization_msgs::msg::Marker mk;
+    mk.header.stamp = stamp;


This could be set directly from rclcpp::Clock().now()

henningkayser · 2019-04-11T17:24:19Z

moveit_core/constraint_samplers/src/default_constraint_samplers.cpp

-      ROS_ERROR_NAMED("constraint_samplers", "IK solver failed with error %d", error.val);
-    else if (verbose_)
-      ROS_INFO_NAMED("constraint_samplers", "IK failed");
+        error.val != moveit_msgs::msg::MoveItErrorCodes::TIMED_OUT){


clang-format

henningkayser · 2019-04-11T17:27:54Z

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp

-    loop_time = (ros::Time::now() - start_time).toSec();
+      RCLCPP_WARN(LOGGER, "%d, %f", count, q_init(free_angle_));
+    time = rclcpp::Clock().now();
+    loop_time = time.seconds() - start_time.seconds();


Suggested change

loop_time = time.seconds() - start_time.seconds();

loop_time = (rclcpp::Clock().now() - start_time).seconds();

Then we don't need to introduce time.

henningkayser · 2019-04-11T17:32:27Z

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h

@@ -92,7 +92,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos
 #if KDL_VERSION_LESS(1, 4, 0)
  void updateInternalDataStructures();
 #else
-  void updateInternalDataStructures() override;
+  void updateInternalDataStructures();


Why should override be removed? This condition shouldn't be broken by the port and we should try to avoid loosening these kind of checks. Also, the if/else block is redundant with this change.

updateInternalDataStructures depends from orocos_kdl, we can see it on melodic here http://docs.ros.org/melodic/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#a8c6fe8ec5cd73c65699143e4a5df9bef, the problem is, that this function is not implemented on ros2, looking into https://github.com/ros2/orocos_kinematics_dynamics/tree/ros2/orocos_kdl it seems like is no more needed. This function is used only on the tests, and it seems like the function is empty.

I removed the entire function and the define here: 02f0eb3

henningkayser · 2019-04-11T17:33:52Z

moveit_core/constraint_samplers/CMakeLists.txt

-    ${urdfdom_headers_LIBRARIES}
-  )
-endif()
+# if(BUILD_TESTING)


I think it's fine to uncomment tests and just let them fail for now so we can fix them later.

@vmayoral Let's get the test fixes cherry picked in

It's already uncommented.

https://github.com/AcutronicRobotics/moveit2/blob/constraint-samplers/moveit_core/constraint_samplers/CMakeLists.txt#L36-L81

ahcorde · 2019-05-24T10:44:40Z

@mlautman Done!

mlautman · 2019-05-29T19:32:08Z

moveit_core/constraint_samplers/CMakeLists.txt

+	if(WIN32)
+	  # set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
+	else()
+	  set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint")


Hardcoding relative paths is a bad idea.

I would recommend that for every library in this package that is used else ware you set a <lib_name>_lib_dir environment variable that can then be used by the tests

A good example is the rcl_lib_dir I have taken the time to show how it is used below:

/home/mike/ws_ros2/src/ros2/rcl/rcl/CMakeLists.txt: 78 79: # rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so 80 # the librcl that they link against is on the library path. 81 # This is especially important on Windows. 82 # This is overwritten each loop, but which one it points to doesn't really matter. 83: set(rcl_lib_dir "$<TARGET_FILE_DIR:${PROJECT_NAME}>") 84 /home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt: 16 17: set(extra_lib_dirs "${rcl_lib_dir}") 18 /home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt: 34 rcl_add_custom_gtest(test_client${target_suffix} 35 SRCS rcl/test_client.cpp 36: INCLUDE_DIRS ${osrf_testing_tools_cpp_INCLUDE_DIRS} 37 ENV ${rmw_implementation_env_var} 38 APPEND_LIBRARY_DIRS ${extra_lib_dirs}

The example uses rcl_add_custom_gtest but it should work just as well with ament_add_gtest

If you don't mind I prefer to merge this. And then I will fix them all together. Because we have other merged that use this style. I can open an issue to avoid forgetting it.

mlautman · 2019-05-29T19:32:23Z

moveit_core/constraint_samplers/CMakeLists.txt

    test/test_constraint_samplers.cpp
    test/pr2_arm_kinematics_plugin.cpp
    test/pr2_arm_ik.cpp
  )
+
+	target_include_directories(test_constraint_samplers PUBLIC


fix whitespace

mlautman · 2019-05-29T19:34:08Z

moveit_core/constraint_samplers/src/constraint_sampler.cpp

@@ -36,13 +36,15 @@

 #include <moveit/constraint_samplers/constraint_sampler.h>

+rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers");


Refactor to put this inside namespace. the methods themselves could be put in a single large namespace block as well

mlautman · 2019-05-29T19:34:51Z

moveit_core/constraint_samplers/src/default_constraint_samplers.cpp

@@ -39,6 +39,8 @@
 #include <cassert>
 #include <boost/bind.hpp>

+rclcpp::Logger LOGGER_DEFAULT_CONTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers");


put Inside namespace

On ROS2 is not implemented, for melodic we can get it on http://docs.ros.org/melodic/api/orocos_kdl/html/classKDL_1_1ChainJntToJacSolver.html#a8c6fe8ec5cd73c65699143e4a5df9bef, looking into ros2 code, there is not similar function https://github.com/ros2/orocos_kinematics_dynamics/tree/ros2/orocos_kdl

anasarrak · 2019-06-13T09:33:09Z

@mlautman and @henningkayser review the changes please

henningkayser

@anasarrak Just some small changes left, then +1 from me

henningkayser · 2019-06-13T14:37:54Z

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp

    error_code.val = error_code.PLANNING_FAILED;
    return false;
  }
-  KDL::Frame pose_desired;
-  tf2::fromMsg(ik_pose, pose_desired);
+  KDL::Frame pose_desired = KDL::Frame(KDL::Rotation::Quaternion(ik_pose.orientation.x, ik_pose.orientation.y,


This should use fromMsg() from tf2_kdl.h once geometry2 is ported to ROS2. Please add a TODO

There is actually an implementation for such transformation on
https://github.com/ros2/geometry2/blob/234725f30ade83caa4bcf019ee854199aa31bef5/tf2_kdl/include/tf2_kdl/tf2_kdl.h#L273-L282, you have to pass a PoseStamped instead of a pose, so I suggest the following changes.

Suggested change

KDL::Frame pose_desired = KDL::Frame(KDL::Rotation::Quaternion(ik_pose.orientation.x, ik_pose.orientation.y,

geometry_msgs::msg::PoseStamped ik_pose_stamped;

ik_pose_stamped.pose = ik_pose;

tf2::Stamped<KDL::Frame> pose_desired;

tf2::fromMsg(ik_pose_stamped, pose_desired);

looks good to me

@anasarrak I'm going to merge this but I would like to see this in a follow up PR

henningkayser · 2019-06-13T14:39:05Z

moveit_core/constraint_samplers/test/test_constraint_samplers.cpp

@@ -673,8 +673,8 @@ TEST_F(LoadPlanningModelsPr2, PoseConstraintSamplerManager)
    if (s->sample(ks, ks_const, 1))
      succ++;
  }
-  ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",
-           (double)succ / (double)NT);
+  // ROS_INFO("Success rate for IK Constraint Sampler with position & orientation constraints for one arm: %lf",


This info should be printed, either by a logger or using std::cout.

AcutronicRobotics@05ad105

henningkayser · 2019-06-13T14:40:49Z

moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp

@@ -61,16 +63,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
  if (!jmg)
    return constraint_samplers::ConstraintSamplerPtr();
  std::stringstream ss;
-  ss << constr;
-  ROS_DEBUG_NAMED("constraint_samplers",
+  ss << constr.name;


I still think that the full constraint should be printed here instead of only the name.

moveit_core/constraint_samplers/CMakeLists.txt

moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp

moveit_core/constraint_samplers/src/default_constraint_samplers.cpp

moveit_core/constraint_samplers/src/union_constraint_sampler.cpp

moveit_core/constraint_samplers/test/pr2_arm_ik.cpp

moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp

moveit_core/constraint_samplers/test/test_constraint_samplers.cpp

mlautman

see review

anasarrak · 2019-06-14T07:54:02Z

@mlautman look at the changes

anasarrak · 2019-06-21T07:25:09Z

Ping @mlautman @henningkayser

ahcorde · 2019-06-24T07:36:30Z

friendly ping @mlautman @henningkayser @davetcoleman

…r_manager_loader Port constraint_sampler_manager_loader to ROS2

vmayoral added 2 commits March 30, 2019 23:28

Port constraint_samplers to ROS 2

3403b96

constraint_samplers, adapt logger according to moveit#21

abd451d

vmayoral mentioned this pull request Apr 8, 2019

Planning interface, port to ROS 2 #61

Merged

henningkayser reviewed Apr 11, 2019

View reviewed changes

ahcorde and others added 6 commits May 24, 2019 12:37

moveit_core constraint-samplers - library as shared

8b3fdcc

Fixing constraint samplers tests

c48a10a

Fixing constraint_samplers tests

a58c4d9

Fixes for building in OS X

e2e067e

moveit_core constraint_samplers - fixing logger variables

f9be4c1

Merge branch 'master' into constraint-samplers

d71434a

ahcorde mentioned this pull request May 24, 2019

Moveit core CMakeLists.txt #90

Closed

14 tasks

mlautman reviewed May 29, 2019

View reviewed changes

ahcorde and others added 9 commits May 29, 2019 23:51

moveit_core constraint_samples - Fix whitespace in CMakeLists.txt

e7aa185

Fixing endtime to use it inline

c8fb7c1

fix passing clock.now to stamp

082b80f

Fix loop_time

a527755

Reverting back override to pr2_arm_kinematics_plugin

c7a3a04

Move constraint_sampler logger to the appropriate namespace

1611306

Moving default_contraint_sampler logger into the namespace

206c27d

clang-format

322d452

anasarrak mentioned this pull request Jun 13, 2019

Update moveit core AcutronicRobotics/moveit2#115

Merged

henningkayser reviewed Jun 13, 2019

View reviewed changes

Update logger test_constraint_samplers

05ad105