Skip to content

Instantly share code, notes, and snippets.

@avoelz
Last active April 27, 2018 10:40
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save avoelz/5095547bb3873d5549137a2e9eb3f883 to your computer and use it in GitHub Desktop.
Save avoelz/5095547bb3873d5549137a2e9eb3f883 to your computer and use it in GitHub Desktop.
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