twist_mux.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (CC BY-NC-SA 4.0 License)
00003  *
00004  *  Copyright (c) 2014, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  This work is licensed under the Creative Commons
00008  *  Attribution-NonCommercial-ShareAlike 4.0 International License.
00009  *
00010  *  To view a copy of this license, visit
00011  *  http://creativecommons.org/licenses/by-nc-sa/4.0/
00012  *  or send a letter to
00013  *  Creative Commons, 444 Castro Street, Suite 900,
00014  *  Mountain View, California, 94041, USA.
00015  *********************************************************************/
00016 
00017 /*
00018  * @author Enrique Fernandez
00019  * @author Siegfried Gevatter
00020  */
00021 
00022 #include <twist_mux/twist_mux.h>
00023 #include <twist_mux/topic_handle.h>
00024 #include <twist_mux/twist_mux_diagnostics.h>
00025 #include <twist_mux/twist_mux_diagnostics_status.h>
00026 #include <twist_mux/utils.h>
00027 #include <twist_mux/xmlrpc_helpers.h>
00028 
00036 bool hasIncreasedAbsVelocity(const geometry_msgs::Twist& old_twist, const geometry_msgs::Twist& new_twist)
00037 {
00038   const auto old_linear_x = std::abs(old_twist.linear.x);
00039   const auto new_linear_x = std::abs(new_twist.linear.x);
00040 
00041   const auto old_angular_z = std::abs(old_twist.angular.z);
00042   const auto new_angular_z = std::abs(new_twist.angular.z);
00043 
00044   return (old_linear_x  < new_linear_x ) or
00045          (old_angular_z < new_angular_z);
00046 }
00047 
00048 namespace twist_mux
00049 {
00050 
00051 TwistMux::TwistMux(int window_size)
00052 {
00053   ros::NodeHandle nh;
00054   ros::NodeHandle nh_priv("~");
00055 
00057   velocity_hs_ = boost::make_shared<velocity_topic_container>();
00058   lock_hs_     = boost::make_shared<lock_topic_container>();
00059   getTopicHandles(nh, nh_priv, "topics", *velocity_hs_);
00060   getTopicHandles(nh, nh_priv, "locks" , *lock_hs_ );
00061 
00063   cmd_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel_out", 1);
00064 
00066   diagnostics_ = boost::make_shared<diagnostics_type>();
00067   status_      = boost::make_shared<status_type>();
00068   status_->velocity_hs = velocity_hs_;
00069   status_->lock_hs     = lock_hs_;
00070 
00071   diagnostics_timer_ = nh.createTimer(ros::Duration(DIAGNOSTICS_PERIOD), &TwistMux::updateDiagnostics, this);
00072 }
00073 
00074 TwistMux::~TwistMux()
00075 {}
00076 
00077 void TwistMux::updateDiagnostics(const ros::TimerEvent& event)
00078 {
00079   status_->priority = getLockPriority();
00080   diagnostics_->updateStatus(status_);
00081 }
00082 
00083 void TwistMux::publishTwist(const geometry_msgs::TwistConstPtr& msg)
00084 {
00085   cmd_pub_.publish(*msg);
00086 }
00087 
00088 template<typename T>
00089 void TwistMux::getTopicHandles(ros::NodeHandle& nh, ros::NodeHandle& nh_priv, const std::string& param_name, std::list<T>& topic_hs)
00090 {
00091   try
00092   {
00093     xh::Array output;
00094     xh::fetchParam(nh_priv, param_name, output);
00095 
00096     xh::Struct output_i;
00097     std::string name, topic;
00098     double timeout;
00099     int priority;
00100     for (int i = 0; i < output.size(); ++i)
00101     {
00102       xh::getArrayItem(output, i, output_i);
00103 
00104       xh::getStructMember(output_i, "name"    , name    );
00105       xh::getStructMember(output_i, "topic"   , topic   );
00106       xh::getStructMember(output_i, "timeout" , timeout );
00107       xh::getStructMember(output_i, "priority", priority);
00108 
00109       topic_hs.emplace_back(nh, name, topic, timeout, priority, this);
00110     }
00111   }
00112   catch (const xh::XmlrpcHelperException& e)
00113   {
00114     ROS_FATAL_STREAM("Error parsing params: " << e.what());
00115   }
00116 }
00117 
00118 int TwistMux::getLockPriority()
00119 {
00120   LockTopicHandle::priority_type priority = 0;
00121 
00124   for (const auto& lock_h : *lock_hs_)
00125   {
00126     if (lock_h.isLocked())
00127     {
00128       auto tmp = lock_h.getPriority();
00129       if (priority < tmp)
00130       {
00131         priority = tmp;
00132       }
00133     }
00134   }
00135 
00136   ROS_DEBUG_STREAM("Priority = " << static_cast<int>(priority));
00137 
00138   return priority;
00139 }
00140 
00141 bool TwistMux::hasPriority(const VelocityTopicHandle& twist)
00142 {
00143   const auto lock_priority = getLockPriority();
00144 
00145   LockTopicHandle::priority_type priority = 0;
00146   std::string velocity_name = "NULL";
00147 
00150   for (const auto& velocity_h : *velocity_hs_)
00151   {
00152     if (not velocity_h.isMasked(lock_priority))
00153     {
00154       const auto velocity_priority = velocity_h.getPriority();
00155       if (priority < velocity_priority)
00156       {
00157         priority = velocity_priority;
00158         velocity_name = velocity_h.getName();
00159       }
00160     }
00161   }
00162 
00163   return twist.getName() == velocity_name;
00164 }
00165 
00166 } // namespace twist_mux


twist_mux
Author(s): Enrique Fernandez , Siegfried-A. Gevatter Pujals
autogenerated on Mon Mar 14 2016 10:21:12