trajectory_monitor.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/planning_scene_monitor/trajectory_monitor.h>
00038 #include <moveit/trajectory_processing/trajectory_tools.h>
00039 #include <ros/rate.h>
00040 #include <limits>
00041 
00042 planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr &state_monitor, double sampling_frequency) :
00043   current_state_monitor_(state_monitor),
00044   trajectory_(current_state_monitor_->getRobotModel(), ""),
00045   sampling_frequency_(5.0)
00046 {
00047   setSamplingFrequency(sampling_frequency);
00048 }
00049 
00050 planning_scene_monitor::TrajectoryMonitor::~TrajectoryMonitor()
00051 {
00052   stopTrajectoryMonitor();
00053 }
00054 
00055 void planning_scene_monitor::TrajectoryMonitor::setSamplingFrequency(double sampling_frequency)
00056 {
00057   if (sampling_frequency <= std::numeric_limits<double>::epsilon())
00058     ROS_ERROR("The sampling frequency for trajectory states should be positive");
00059   else
00060     sampling_frequency_ = sampling_frequency;
00061 }
00062 
00063 bool planning_scene_monitor::TrajectoryMonitor::isActive() const
00064 {
00065   return record_states_thread_;
00066 }
00067 
00068 void planning_scene_monitor::TrajectoryMonitor::startTrajectoryMonitor()
00069 {
00070   if (!record_states_thread_)
00071   {
00072     record_states_thread_.reset(new boost::thread(boost::bind(&TrajectoryMonitor::recordStates, this)));
00073     ROS_DEBUG("Started trajectory monitor");
00074   }
00075 }
00076 
00077 void planning_scene_monitor::TrajectoryMonitor::stopTrajectoryMonitor()
00078 {
00079   if (record_states_thread_)
00080   {
00081     boost::scoped_ptr<boost::thread> copy;
00082     copy.swap(record_states_thread_);
00083     copy->join();
00084     ROS_DEBUG("Stopped trajectory monitor");
00085   }
00086 }
00087 
00088 void planning_scene_monitor::TrajectoryMonitor::clearTrajectory()
00089 {
00090   bool restart = isActive();
00091   if (restart)
00092     stopTrajectoryMonitor();
00093   trajectory_.clear();
00094   if (restart)
00095     startTrajectoryMonitor();
00096 }
00097 
00098 void planning_scene_monitor::TrajectoryMonitor::recordStates()
00099 {
00100   if (!current_state_monitor_)
00101     return;
00102 
00103   ros::Rate rate(sampling_frequency_);
00104 
00105   while (record_states_thread_)
00106   {
00107     rate.sleep();
00108     std::pair<robot_state::RobotStatePtr, ros::Time> state = current_state_monitor_->getCurrentStateAndTime();
00109     if (trajectory_.empty())
00110     {
00111       trajectory_.addSuffixWayPoint(state.first, 0.0);
00112       trajectory_start_time_ = state.second;
00113       last_recorded_state_time_ = state.second;
00114     }
00115     else
00116     {
00117       trajectory_.addSuffixWayPoint(state.first, (state.second - last_recorded_state_time_).toSec());
00118       last_recorded_state_time_ = state.second;
00119     }
00120     if (state_add_callback_)
00121       state_add_callback_(state.first, state.second);
00122   }
00123 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 02:31:40