Port robot_trajectory to ROS2 #39

vmayoral · 2019-03-10T17:04:30Z

No description provided.

@anasarrak

moveit_core/robot_trajectory/CMakeLists.txt

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

mlautman · 2019-03-12T00:36:33Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -101,11 +102,6 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st
 void RobotTrajectory::reverse()
 {
  std::reverse(waypoints_.begin(), waypoints_.end());
-  for (robot_state::RobotStatePtr& waypoint : waypoints_)


Why are we removing this?

@anasarrak I don't quite recall having removed this on purpose. Does something come to mind for you? @mlautman maybe this is the result of syncing with moveit1?

This was recently added in moveit1, yes:
https://github.com/ros-planning/moveit/pull/1335/files#diff-93f52a3b52efb75e0963646beb76dfcfR104

please restore.

@davetcoleman, not sure where this is coming from but reverting this as you pointed causes yet more problems when compiling:

/usr/local/include/octomap/OccupancyOcTreeBase.hxx:116:108: warning: unused parameter 'maxrange' [-Wunused-parameter] void OccupancyOcTreeBase<NODE>::insertPointCloudRays(const Pointcloud& pc, const point3d& origin, double maxrange, bool lazy_eval) { ^ /Users/victor/ros2_moveit_ws/src/moveit2/moveit_core/robot_trajectory/src/robot_trajectory.cpp:108:15: error: no member named 'invertVelocity' in 'moveit::core::RobotState' waypoint->invertVelocity(); ~~~~~~~~ ^

I'd suggest we finalize the ROS 2 porting and then we rebase things with respect MoveIt! (1).

How shall we proceed about this?

The issue here is that invertVelocity() has not yet been synced to the PRs base branch but the current master branch has this function (https://github.com/ros-planning/moveit2/pull/59/files#diff-15d4dc0e9e1eee143f7576803dfe8ea2L441). Did you try compiling these changes after a rebase to master? Actually it's the same problem as in PR #59 where invertVelocity() was accidently removed from the feature branch (https://github.com/ros-planning/moveit2/pull/59/files#diff-15d4dc0e9e1eee143f7576803dfe8ea2L441).

moveit_core/robot_trajectory/src/robot_trajectory.cpp

mlautman · 2019-03-12T00:46:46Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

  for (std::size_t i = 0; i < waypoints_.size(); ++i)
  {
    if (duration_from_previous_.size() > i)
      total_time += duration_from_previous_[i];
-
+      int seconds = (int)total_time;
+      dur_total = rclcpp::Duration((int32_t)seconds, (int32_t)((total_time-seconds)*1.0e+9));


Would it be better to define a method for converting from a double to rclcpp::Duration? This logic takes a minute to process in context.

One option would be to pass in the total nanoseconds as a rcl_duration_value_t which is really just an int64_t.

Can you help with this @mlautman?

I believe that all you would need is:

#include "rcl/time.h" rclcpp::Duration time(static_cast<rcl_duration_value_t>time_in_nanoseconds);

Although given that rcl_duration_value_t is an int64_t

# From rcl/time.h and rcutils/time.h typedef int64_t rcutils_duration_value_t; typedef rcutils_duration_value_t rcl_duration_value_t;

you should be able to simply do:

rclcpp::Duration time(static_cast<int64_t>time_in_nanoseconds);

Let me know if that doesn't work

Hello @mlautman, tried your recommendations but failed miserably and didn't manage to get it compiling. Maybe I simply didn't understood what you meant?

Can you please make a code suggestion that compiles?

moveit_core/robot_trajectory/src/robot_trajectory.cpp

davetcoleman · 2019-03-19T18:59:26Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -101,11 +102,6 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st
 void RobotTrajectory::reverse()
 {
  std::reverse(waypoints_.begin(), waypoints_.end());
-  for (robot_state::RobotStatePtr& waypoint : waypoints_)


This was recently added in moveit1, yes:
https://github.com/ros-planning/moveit/pull/1335/files#diff-93f52a3b52efb75e0963646beb76dfcfR104

please restore.

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

Misalignment when merging things with moveit1 moveit#39 (comment)

henningkayser · 2019-04-09T16:50:23Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp


  for (std::size_t i = 0; i < state_count; ++i)
  {
-    this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
+    this_time_stamp = trajStamp + rclcpp::Duration(trajectory.points[i].time_from_start.sec,


Isn't time_from_start already an rclcpp::Duration so we can just add it directly?

henningkayser · 2019-04-09T16:53:44Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

  for (std::size_t i = 0; i < waypoints_.size(); ++i)
  {
    if (duration_from_previous_.size() > i)
      total_time += duration_from_previous_[i];
-
+    int seconds = static_cast<int>(total_time);
+    dur_total = rclcpp::Duration((int32_t)seconds, (int32_t)((total_time - seconds) * 1.0e+9));


Maybe just a nit, but here we could also just create a rclcpp::Duration() of 1 second and multiply it with total_time.

henningkayser · 2019-04-09T16:55:14Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

-      this_time_stamp =
-          trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
+      this_time_stamp = rclcpp::Time(trajectory.joint_trajectory.header.stamp) +
+                        rclcpp::Duration(trajectory.joint_trajectory.points[i].time_from_start.sec,


Just add stamp and time_from_start directly?

henningkayser · 2019-04-09T16:55:22Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

-      this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
-                        trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
+      this_time_stamp = rclcpp::Time(trajectory.multi_dof_joint_trajectory.header.stamp) +
+                        rclcpp::Duration(trajectory.multi_dof_joint_trajectory.points[i].time_from_start.sec,


ahcorde · 2019-05-06T21:42:10Z

I tackled some of the issues. Please review it

henningkayser

Looks good! I would vote for committing my suggested change and then lgtm.

henningkayser · 2019-05-08T16:50:29Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

  for (std::size_t i = 0; i < waypoints_.size(); ++i)
  {
    if (duration_from_previous_.size() > i)
      total_time += duration_from_previous_[i];

+    dur_total = rclcpp::Duration(static_cast<int64_t>(total_time*1e+9));


I meant something like this:

Suggested change

dur_total = rclcpp::Duration(static_cast<int64_t>(total_time*1e+9));

dur_total = rclcpp::Duration(1, 0) * total_time;

I still think it's odd that rclcpp::Time and rclcpp::Duration don't have double constructors but to me this version would be more readable.

Commited! ;)

moveit_core/robot_trajectory/src/robot_trajectory.cpp

davetcoleman · 2019-05-16T21:31:02Z

Otherwise looks good!

That seems right https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#id3 Co-Authored-By: Dave Coleman <dave@picknik.ai>

Accepting as well. To take into account https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#id3 for future reference Co-Authored-By: Dave Coleman <dave@picknik.ai>

ahcorde · 2019-05-17T08:46:00Z

@davetcoleman can we merge this?

ahcorde · 2019-05-20T10:08:31Z

@davetcoleman friendly ping

davetcoleman

Great!

…nitor Port Planning scene monitor

Port robot_trajectory to ROS2

80ee295

Authored by @anasarrak

mlautman reviewed Mar 12, 2019

View reviewed changes

vmayoral added 2 commits March 17, 2019 00:33

robot_trajectory, use static_cast

60a8e9c

Compress the use of .seconds()

e6e7a0b

vmayoral mentioned this pull request Mar 16, 2019

Port collision_detection to ROS2 #40

Merged

robot_trajectory, clang format

9cb2a18

davetcoleman reviewed Mar 19, 2019

View reviewed changes

vmayoral added a commit to AcutronicRobotics/moveit2 that referenced this pull request Mar 30, 2019

robot_trajectory, restore changes from moveit1

000343f

Misalignment when merging things with moveit1 moveit#39 (comment)

vmayoral mentioned this pull request Apr 8, 2019

Planning interface, port to ROS 2 #61

Merged

henningkayser reviewed Apr 9, 2019

View reviewed changes

ahcorde added 2 commits May 6, 2019 23:38

udpating robot_trajectory CMakeLists.txt

c08749e

Restoring accidently removed code and fix time convertions

3293633

henningkayser reviewed May 8, 2019

View reviewed changes

Robot_trajectory - constructor of rclcpp::Duration more readable

f0c522b

davetcoleman reviewed May 16, 2019

View reviewed changes

moveit_core/robot_trajectory/src/robot_trajectory.cpp Outdated Show resolved Hide resolved

moveit_core/robot_trajectory/src/robot_trajectory.cpp Outdated Show resolved Hide resolved

Víctor Mayoral Vilches and others added 2 commits May 16, 2019 23:42

Update moveit_core/robot_trajectory/src/robot_trajectory.cpp

ff4b888

That seems right https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#id3 Co-Authored-By: Dave Coleman <dave@picknik.ai>

Update moveit_core/robot_trajectory/src/robot_trajectory.cpp

5936b7a

Accepting as well. To take into account https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#id3 for future reference Co-Authored-By: Dave Coleman <dave@picknik.ai>

davetcoleman approved these changes May 21, 2019

View reviewed changes

davetcoleman merged commit cd585ee into moveit:master May 21, 2019

JafarAbdi pushed a commit to JafarAbdi/moveit2 that referenced this pull request Oct 28, 2019

Merge pull request moveit#39 from AcutronicRobotics/planning_scene_mo…

6235fb1

…nitor Port Planning scene monitor

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Port robot_trajectory to ROS2 #39

Port robot_trajectory to ROS2 #39

vmayoral commented Mar 10, 2019

mlautman Mar 12, 2019

vmayoral Mar 16, 2019

davetcoleman Mar 19, 2019

vmayoral Mar 30, 2019

vmayoral Mar 30, 2019

henningkayser Apr 9, 2019

mlautman Mar 12, 2019

mlautman Mar 12, 2019

vmayoral Mar 16, 2019

mlautman Mar 20, 2019

mlautman Mar 20, 2019

vmayoral Mar 30, 2019

davetcoleman Mar 19, 2019

henningkayser Apr 9, 2019

henningkayser Apr 9, 2019

henningkayser Apr 9, 2019

henningkayser Apr 9, 2019

ahcorde commented May 6, 2019

henningkayser left a comment

henningkayser May 8, 2019

ahcorde May 16, 2019

davetcoleman commented May 16, 2019

ahcorde commented May 17, 2019

ahcorde commented May 20, 2019

davetcoleman left a comment

	dur_total = rclcpp::Duration(static_cast<int64_t>(total_time*1e+9));
	dur_total = rclcpp::Duration(1, 0) * total_time;

Port robot_trajectory to ROS2 #39

Port robot_trajectory to ROS2 #39

Conversation

vmayoral commented Mar 10, 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

Choose a reason for hiding this comment

ahcorde commented May 6, 2019

henningkayser left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented May 16, 2019

ahcorde commented May 17, 2019

ahcorde commented May 20, 2019

davetcoleman left a comment

Choose a reason for hiding this comment