Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port planning_scene_monitor to ROS2 #112

Merged
merged 5 commits into from
Jan 27, 2020
Merged

Port planning_scene_monitor to ROS2 #112

merged 5 commits into from
Jan 27, 2020

Conversation

JafarAbdi
Copy link
Member

No description provided.

henningkayser
henningkayser reviewed Oct 29, 2019
View reviewed changes
Copy link
Member

@henningkayser henningkayser left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm only half way through. I'll give this a second round tomorrow

moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/CMakeLists.txt Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
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))));
// TODO (anasarrak): replace this for the appropiate function, there is no similar
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There should be one now

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

They replaced it see, I should change MoveIt to use the new API and will port it to MoveIt2

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Show resolved Hide resolved
@RoboticsYY RoboticsYY force-pushed the pr-planning_scene_monitor branch from a85908f to c96c9ba Compare December 19, 2019 03:36
@RoboticsYY RoboticsYY mentioned this pull request Dec 20, 2019
Merged
@RoboticsYY RoboticsYY force-pushed the pr-planning_scene_monitor branch 9 times, most recently from e8bfb9e to a4e5721 Compare January 15, 2020 10:32
henningkayser
henningkayser reviewed Jan 15, 2020
View reviewed changes
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/demos/demo_scene.cpp Outdated Show resolved Hide resolved
...lanning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
(robot_description_[0] == '/') ? robot_description_.substr(1) : robot_description_;

nh_.param(robot_description + "_planning/default_robot_padding", default_robot_padd_, 0.0);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These have direct corresponding functions, we don't need to check has_parameter everywhere.

moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp Outdated Show resolved Hide resolved
@RoboticsYY RoboticsYY force-pushed the pr-planning_scene_monitor branch 3 times, most recently from b8fdfd7 to ad3e12b Compare January 20, 2020 14:55
@henningkayser
Copy link
Member

@RoboticsYY thanks for fixing this up so far! Could you squash/cleanup the commit history and address the remaining points? Then lgtm

@JafarAbdi JafarAbdi mentioned this pull request Jan 22, 2020
Merged
7 tasks
@RoboticsYY RoboticsYY force-pushed the pr-planning_scene_monitor branch 2 times, most recently from 70d8bf2 to 1f46a61 Compare January 24, 2020 09:21
RoboticsYY
RoboticsYY reviewed Jan 26, 2020
View reviewed changes
Copy link
Contributor

@RoboticsYY RoboticsYY left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@JafarAbdi I have left some comments to the code I think I haven't finished clean-up or found an answer to.

...anning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h Outdated
ros::Subscriber collision_object_subscriber_;
rclcpp::Subscription<moveit_msgs::msg::AttachedCollisionObject>::SharedPtr attached_collision_object_subscriber_;
rclcpp::Subscription<moveit_msgs::msg::CollisionObject>::SharedPtr collision_object_subscriber_;
std::unique_ptr<tf2_ros::MessageFilter<moveit_msgs::msg::CollisionObject> > collision_object_filter_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

collision_object_filter seems not necessary.

...anning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
@@ -544,21 +539,22 @@ class PlanningSceneMonitor : private boost::noncopyable

/// the amount of time to wait in between updates to the robot state
// This field is protected by state_pending_mutex_
ros::WallDuration dt_state_update_;
std::chrono::duration<double> dt_state_update_; // 1hz
Copy link
Contributor

@RoboticsYY RoboticsYY Jan 26, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe there should be some global guidance about replacing ros::WallTime and ros::WallDuration by std::chrono::system_clock::time_point and std::chrono::duration.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree we should add an alias for that something like moveit::TimePoint

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
...anning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
// TODO: (anasarrak) callbacks on ROS2?
// https://answers.ros.org/question/300874/how-do-you-use-callbackgroups-as-a-replacement-for-callbackqueues-in-ros2/
// ros::CallbackQueue queue_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> spinner_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

spinner_ seems not necessary.

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp Outdated Show resolved Hide resolved
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
else
{
ROS_DEBUG_NAMED(LOGNAME, "Reading additional default collision operations");

XmlRpc::XmlRpcValue coll_ops;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Refactor for XmlRpc.

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
node_->get_parameter_or(robot_description + "_planning/default_attached_padding", default_attached_padd_, 0.0);
default_robot_link_padd_ = std::map<std::string, double>();
default_robot_link_scale_ = std::map<std::string, double>();
// TODO: enable parameter type support to std::map
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Seems ROS2 doesn't support parameter type for std::map.

JafarAbdi
JafarAbdi commented Jan 27, 2020
View reviewed changes
moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
acm.setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]),
std::string(coll_ops[i]["operation"]) == "disable");
}
RCLCPP_DEBUG(LOGGER, "Reading additional default collision operations");
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this part important anymore, after googling a little bit seems to me this was part of the arm_navigation package .? see 1 2

@JafarAbdi JafarAbdi force-pushed the pr-planning_scene_monitor branch from ab2cf37 to 37a5081 Compare January 27, 2020 18:51
@JafarAbdi JafarAbdi force-pushed the pr-planning_scene_monitor branch from 37a5081 to aaf8189 Compare January 27, 2020 18:56
@JafarAbdi JafarAbdi force-pushed the pr-planning_scene_monitor branch from aaf8189 to 0a10aa6 Compare January 27, 2020 18:58
@JafarAbdi JafarAbdi force-pushed the pr-planning_scene_monitor branch from 0a10aa6 to 08e006b Compare January 27, 2020 19:18
@henningkayser henningkayser merged commit 3080347 into moveit:master Jan 27, 2020
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@RoboticsYY RoboticsYY RoboticsYY left review comments

@henningkayser henningkayser henningkayser approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
3 participants
@JafarAbdi @henningkayser @RoboticsYY