Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00028
00029
00030
00031
00032 #include <algorithm>
00033 #include <cstddef>
00034
00035 #include "joint_state_controller/joint_state_controller.h"
00036
00037 namespace joint_state_controller
00038 {
00039
00040 bool JointStateController::init(hardware_interface::JointStateInterface* hw,
00041 ros::NodeHandle& root_nh,
00042 ros::NodeHandle& controller_nh)
00043 {
00044
00045 const std::vector<std::string>& joint_names = hw->getNames();
00046 num_hw_joints_ = joint_names.size();
00047 for (unsigned i=0; i<num_hw_joints_; i++)
00048 ROS_DEBUG("Got joint %s", joint_names[i].c_str());
00049
00050
00051 if (!controller_nh.getParam("publish_rate", publish_rate_)){
00052 ROS_ERROR("Parameter 'publish_rate' not set");
00053 return false;
00054 }
00055
00056
00057 realtime_pub_.reset(new realtime_tools::RealtimePublisher<sensor_msgs::JointState>(root_nh, "joint_states", 4));
00058
00059
00060 for (unsigned i=0; i<num_hw_joints_; i++){
00061 joint_state_.push_back(hw->getHandle(joint_names[i]));
00062 realtime_pub_->msg_.name.push_back(joint_names[i]);
00063 realtime_pub_->msg_.position.push_back(0.0);
00064 realtime_pub_->msg_.velocity.push_back(0.0);
00065 realtime_pub_->msg_.effort.push_back(0.0);
00066 }
00067 addExtraJoints(controller_nh, realtime_pub_->msg_);
00068
00069 return true;
00070 }
00071
00072 void JointStateController::starting(const ros::Time& time)
00073 {
00074
00075 last_publish_time_ = time;
00076 }
00077
00078 void JointStateController::update(const ros::Time& time, const ros::Duration& )
00079 {
00080
00081 if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0/publish_rate_) < time){
00082
00083
00084 if (realtime_pub_->trylock()){
00085
00086 last_publish_time_ = last_publish_time_ + ros::Duration(1.0/publish_rate_);
00087
00088
00089
00090
00091 realtime_pub_->msg_.header.stamp = time;
00092 for (unsigned i=0; i<num_hw_joints_; i++){
00093 realtime_pub_->msg_.position[i] = joint_state_[i].getPosition();
00094 realtime_pub_->msg_.velocity[i] = joint_state_[i].getVelocity();
00095 realtime_pub_->msg_.effort[i] = joint_state_[i].getEffort();
00096 }
00097 realtime_pub_->unlockAndPublish();
00098 }
00099 }
00100 }
00101
00102 void JointStateController::stopping(const ros::Time& )
00103 {}
00104
00105 void JointStateController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
00106 {
00107
00108
00109 XmlRpc::XmlRpcValue list;
00110 if (!nh.getParam("extra_joints", list))
00111 {
00112 ROS_DEBUG("No extra joints specification found.");
00113 return;
00114 }
00115
00116 if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
00117 {
00118 ROS_ERROR("Extra joints specification is not an array. Ignoring.");
00119 return;
00120 }
00121
00122 for(std::size_t i = 0; i < list.size(); ++i)
00123 {
00124 if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
00125 {
00126 ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
00127 "'. Ignoring.");
00128 continue;
00129 }
00130
00131 if (!list[i].hasMember("name"))
00132 {
00133 ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
00134 continue;
00135 }
00136
00137 const std::string name = list[i]["name"];
00138 if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
00139 {
00140 ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
00141 continue;
00142 }
00143
00144 const bool has_pos = list[i].hasMember("position");
00145 const bool has_vel = list[i].hasMember("velocity");
00146 const bool has_eff = list[i].hasMember("effort");
00147
00148 const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
00149 if (has_pos && list[i]["position"].getType() != typeDouble)
00150 {
00151 ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
00152 continue;
00153 }
00154 if (has_vel && list[i]["velocity"].getType() != typeDouble)
00155 {
00156 ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
00157 continue;
00158 }
00159 if (has_eff && list[i]["effort"].getType() != typeDouble)
00160 {
00161 ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
00162 continue;
00163 }
00164
00165
00166 const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
00167 const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
00168 const double eff = has_eff ? static_cast<double>(list[i]["effort"]) : 0.0;
00169
00170
00171 msg.name.push_back(name);
00172 msg.position.push_back(pos);
00173 msg.velocity.push_back(vel);
00174 msg.effort.push_back(eff);
00175 }
00176 }
00177
00178 }
00179
00180 PLUGINLIB_EXPORT_CLASS( joint_state_controller::JointStateController, controller_interface::ControllerBase)