-
-
Save avoelz/5095547bb3873d5549137a2e9eb3f883 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp | |
index a5c6b89..a0f0069 100644 | |
--- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp | |
+++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp | |
@@ -51,6 +51,7 @@ void move_group::MoveGroupPlanService::initialize() | |
bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotionPlan::Request& req, | |
moveit_msgs::GetMotionPlan::Response& res) | |
{ | |
+ ros::Time ts = ros::Time::now(); | |
ROS_INFO("Received new planning service request..."); | |
// before we start planning, ensure that we have the latest robot state received... | |
context_->planning_scene_monitor_->waitForCurrentRobotState(ros::Time::now()); | |
@@ -69,7 +70,7 @@ bool move_group::MoveGroupPlanService::computePlanService(moveit_msgs::GetMotion | |
ROS_ERROR("Planning pipeline threw an exception: %s", ex.what()); | |
res.motion_plan_response.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; | |
} | |
- | |
+ ROS_INFO("Elapsed time: %.3f s", (ros::Time::now() - ts).toSec()); | |
return true; | |
} | |
diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp | |
index 0c51cbe..3afc064 100644 | |
--- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp | |
+++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp | |
@@ -333,6 +333,8 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso | |
} | |
bool update = false; | |
+ ROS_INFO_STREAM_THROTTLE(0.1, "planning_scene_monitor::CurrentStateMonitor::jointStateCallback"); | |
+ | |
{ | |
boost::mutex::scoped_lock _(state_update_lock_); | |
// read the received values, and update their time stamps |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment