[WIP] Planning_scene_monitor (current_state_monitor) #29

anasarrak · 2019-03-13T12:59:15Z

No description provided.

It's a still WIP, problems with tf2_buffer_ there is no _removeTransformsChangedListener and _addTransformsChangedListener on geometry2 for ros2, think how to change it. the appropriate change for ros::WallTime is std::chrono::system_clock since it's not implemented yet find something to replace ros::WallDuration

anasarrak · 2019-03-13T13:05:19Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

    if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
    {
      tf_connection_.reset(new TFConnection(
-          tf_buffer_->_addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))));


@davetcoleman and @mlautman I'm trying to replace the _addTransformsChangedListener call since on ros2 geometry this function is not implemented, I would like some input about how to procede with that.

There is a similar function called addTransformableCallback but it has nothing to do with that.

ping @henningkayser, @davetcoleman, @mlautman

Do you know why _addTransformsChangedListener() is not implemented?

@IanTheEngineer made this change 10 months ago, here: moveit/moveit@736251d

Looking at that PR, it required the following PRs to pass:

This PR depends on the following upstream API changes for Melodic:
ros/geometry#163 (Exposing tf2_ros::Buffer object from TransformListener)
ros/geometry2#292 (tf2 Type conversions)
ros/geometry2#294 (Additional tf2 Type conversions)
ros-visualization/rviz#1236 (Exposing tf2 without ROS deprecation warnings)

So likely one of those needs to be forward ported to ROS2.

Do you know why _addTransformsChangedListener() is not implemented?

Look at the moveit/moveit#830 (comment), on the porting procedure they just left it apart as it seems to be "deprecated".

moveit/moveit#830 (comment)

addTransformsChangedListener API has been replaced by the TransformableRequest interface. An example of using it is in the tf2::MessageFilters: http://docs.ros.org/jade/api/tf2_ros/html/c++/message__filter_8h_source.html

@IanTheEngineer can you help shed some light on this question?

ping @mlautman, @davetcoleman

I have to finish this...

@anasarrak I don't know if I have any more information than you do at this point.. Have you had the chance to hunt down the dependencies that Dave referenced to see if any of them might be holding up the port? Perhaps @tfoote's comment can help shed some light here: moveit/moveit#830 (comment)

anasarrak · 2019-03-13T13:06:34Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

      missing_states.push_back(joint->getName());
      result = false;
    }
  }
  return result;
 }

-bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
-{
-  ros::WallTime start = ros::WallTime::now();


@wjwwood As you mentioned on moveit#31 the walltime calls should be replaced with std::chrono::system_clock, so I guess that the correct use of WallDuration should be something like std::chrono::duration or is there a better way to do it, like just using rclcpp::Duration calls?

std::chrono::duration is sufficient I think. We were supposed to have to/from functions for converting between std::chrono::duration and rclcpp::Duration, but I don't know if they got implemented or not. It would be a good thing to contribute if they're missing.

I don't know what the context here is, so I can't advise you. Please give me an exact question or scenario and I can help more.

Basically the question was: What is the best replacemanet for ros::WallDuration/ros::WallTime since they are not implemented for ros2. You have already answered me with, use std::chrono::system_clock as replacement for ros::walltime and std::chrono::duration for ros::WallDuration, right?

Much of the functionality that provides boost are implemented on C++11

davetcoleman · 2019-03-15T01:53:46Z

moveit_ros/planning/CMakeLists.txt

+ament_export_include_directories(include)
+ament_export_libraries(${libraries})
+
+ament_package()


A stray COLCON_IGNORE was committed in the following file:
moveit_ros/planning/COLCON_IGNORE

moveit_ros/planning/planning_scene_monitor/CMakeLists.txt

davetcoleman · 2019-03-15T01:56:31Z

...lanning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h


  /** @brief Get the current state
   *  @return Returns the current state */
  robot_state::RobotStatePtr getCurrentState() const;

+  std::pair<robot_state::RobotStatePtr, rclcpp::Time> getCurnode_rentStateAndTime() const;


the name of this function is broken. also, is this function needed?

Fixed here f12ad0e, thanks for this nice catch on the mispelling (it's still a WIP). Also, it's needed on

moveit2/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp

Line 110 in f12ad0e

std::pair<robot_state::RobotStatePtr, ros::Time> state = current_state_monitor_->getCurrentStateAndTime();

davetcoleman · 2019-03-15T02:01:32Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

    if (tf_buffer_ && !robot_model_->getMultiDOFJointModels().empty())
    {
      tf_connection_.reset(new TFConnection(
-          tf_buffer_->_addTransformsChangedListener(boost::bind(&CurrentStateMonitor::tfCallback, this))));


Do you know why _addTransformsChangedListener() is not implemented?

@IanTheEngineer made this change 10 months ago, here: moveit/moveit@736251d

Looking at that PR, it required the following PRs to pass:

This PR depends on the following upstream API changes for Melodic:
ros/geometry#163 (Exposing tf2_ros::Buffer object from TransformListener)
ros/geometry2#292 (tf2 Type conversions)
ros/geometry2#294 (Additional tf2 Type conversions)
ros-visualization/rviz#1236 (Exposing tf2 without ROS deprecation warnings)

So likely one of those needs to be forward ported to ROS2.

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

Update TODO for the demos Co-Authored-By: anasarrak <anasarrak@gmail.com>

anasarrak · 2019-03-18T10:18:50Z

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

-      RCLCPP_WARN(logger, "default_collision_operations is not an array");
-      return;
-    }
+  for(auto & parameter : parameter_robot_description->get_parameters({robot_description_ + "_planning/default_collision_operations"})){


I would like please some input about these changes @ahcorde

Changes on: -planning_interface: CMakelists updated so it only compiles move_group_interface -move_group_interface: Changes on cmakelist, msgs and commented out all the code, letting available plan and execute, ill be focusing more on these changes once planning_scene_monitor PR is closed. - planning_scene_interface: Changes on the msgs on the .h, still has to be modified the .cpp - py_binding_tools: The cmakelist is prepared to compile with colcon.

vmayoral · 2019-03-30T12:42:58Z

@anasarrak, what's the status of this?

…o move_group

The waitForAction function has some callbacks that has to be reviewed, added some TODO and a logger so it prints something when we launch it

Changed both publishers to rclcpp publishers, Added a TODO, since trajectory_execution_manager is not ported yet, I have hardcoded the topic name

Sync Move group with planning_last

anasarrak · 2019-04-03T14:13:50Z

Due to the conflicts with master I'll split this pr on different PRs, since planning_scene_monitor needs to be ported for the different modules, for example plan_execution, trajectory_execution_manager... I prefer to merge it on master as it is and fix it on the future.

anasarrak added 8 commits March 11, 2019 12:32

Prepare moveit_ros_planning to build for ROS2

226dbd0

Update planning CMakelists to compile collision_plugin_loader

3ae6924

Update collision_plugin_loader CMakelist

184dc6a

Port collision_plugin_loader

df334f8

Update CMakeLists.txt

e40cf9c

Reverta back changes perception CMakelists

cd1a6bd

Prepare cmakelists current_state_monitor

3eb5257

anasarrak requested a review from ahcorde March 13, 2019 12:59

anasarrak commented Mar 13, 2019

View reviewed changes

anasarrak added 4 commits March 13, 2019 16:59

Update waitForCurrentState function adding std::chrono::duration

657fcca

change duration_cast to nanoseconds

556e2dc

Use change boost::mutex to std::unique_lock<std::mutex>

7ebc60e

Replace boost::function to std::function

d9e7292

Much of the functionality that provides boost are implemented on C++11

davetcoleman reviewed Mar 15, 2019

View reviewed changes

davetcoleman and others added 5 commits March 15, 2019 09:35

Update moveit_ros/planning/planning_scene_monitor/CMakeLists.txt

e6c08b9

Update TODO for the demos Co-Authored-By: anasarrak <anasarrak@gmail.com>

current_state_monitor.h miss spelling

f12ad0e

Fix current_state_monitor misspell

84878bf

current_state_monitor, update subscriber to shared

8ccf0e5

Changes on publish/subscribers, logs and services

505dbde

vmayoral pushed a commit that referenced this pull request Mar 16, 2019

Port moveit_msgs services (#29)

f071f98

anasarrak added 2 commits March 18, 2019 11:14

planning_scene_monitor, change parameters on configureCollisionMatrix

359cc6b

planning_scene_monitor Changes on parameters

96ae2b0

anasarrak commented Mar 18, 2019

View reviewed changes

anasarrak added 5 commits March 18, 2019 12:13

Replace the spinner with SingpleThreadedExecutor

84102c1

Update getStateUpdateFrequency time, and add some TODO

06abf5b

planning_scene, Time, params, publishers and canTransform(not finished)

f52951a

Update canTrasform passing TimePoint

f2d3531

Change srv to shared, change planning_scene_world subscriber

d996461

anasarrak added 8 commits March 26, 2019 14:00

Update move_group cmakelists

3622d40

Update moveit_core controller_manager

42866f4

prepare plan_execution to compile

0dd077a

update msg on sensor_manager

bdb37f8

update plan_execution logger and WallDuration

893dda2

Port move_group_capability to ros2

c84f5de

Update planning_scene_interface.cpp

c376479

anasarrak and others added 15 commits April 1, 2019 12:18

Re enable private variables and change getting clock duration

880a8e2

Merge branch 'move_group' of github.com:AcutronicRobotics/moveit2 int…

88d4629

…o move_group

Update waitForAction function for ros2 and rename action variables

59b11d1

The waitForAction function has some callbacks that has to be reviewed, added some TODO and a logger so it prints something when we launch it

Rename geometry_msgs

5a8ad53

Change publishers and action calls

d88515f

Change wallduration to std::chrono on the header

7f6f8c2

Cange service calls

043ac4d

Add the correct declaration to the services

f061ae3

Update query_service (getInterfaceDescription) callback

dfe8291

Update callback

398a033

Enable getRobotModel

1bdc526

Change ros_fatal_stream_named to RCLCPP_FATAL

803fbbd

Change trajectory_event_publisher_ and trajectory_event_publisher_

365eb3f

Changed both publishers to rclcpp publishers, Added a TODO, since trajectory_execution_manager is not ported yet, I have hardcoded the topic name

change action node name

4ee67ff

Merge pull request #37 from AcutronicRobotics/move_group

bbf0f66

Sync Move group with planning_last

anasarrak added 4 commits April 3, 2019 16:21

Add TODO for the tests to collision_detection cmakelist

36b52ba

Disable move_group_interface

3f7b3b3

Remove ros1_bridge

5092e61

Update depends on package.xml (perception)

8e72222

anasarrak closed this Apr 8, 2019

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[WIP] Planning_scene_monitor (current_state_monitor) #29

[WIP] Planning_scene_monitor (current_state_monitor) #29

anasarrak commented Mar 13, 2019

anasarrak Mar 13, 2019

anasarrak Mar 14, 2019

davetcoleman Mar 15, 2019

anasarrak Mar 15, 2019

anasarrak Mar 15, 2019

mlautman Mar 15, 2019

anasarrak Mar 22, 2019

mlautman Mar 25, 2019

anasarrak Mar 13, 2019

wjwwood Mar 15, 2019

anasarrak Mar 18, 2019

davetcoleman Mar 15, 2019

davetcoleman Mar 15, 2019

anasarrak Mar 15, 2019

davetcoleman Mar 15, 2019

anasarrak Mar 18, 2019

vmayoral commented Mar 30, 2019

anasarrak commented Apr 3, 2019

[WIP] Planning_scene_monitor (current_state_monitor) #29

[WIP] Planning_scene_monitor (current_state_monitor) #29

Conversation

anasarrak commented Mar 13, 2019

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

Choose a reason for hiding this comment

vmayoral commented Mar 30, 2019

anasarrak commented Apr 3, 2019