chain_manager.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2014-2016 Fetch Robotics Inc.
00003  * Copyright (C) 2013-2014 Unbounded Robotics Inc.
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 // Author: Michael Ferguson
00019 
00020 #include <robot_calibration/capture/chain_manager.h>
00021 
00022 namespace robot_calibration
00023 {
00024 
00025 ChainManager::ChainManager(ros::NodeHandle& nh, double wait_time)
00026 {
00027   // We cannot do much without some kinematic chains
00028   if (!nh.hasParam("chains"))
00029   {
00030     // TODO raise error
00031   }
00032 
00033   // Get chains
00034   XmlRpc::XmlRpcValue chains;
00035   nh.getParam("chains", chains);
00036   ROS_ASSERT(chains.getType() == XmlRpc::XmlRpcValue::TypeArray);
00037 
00038   // Construct each chain to manage
00039   for (int i = 0; i < chains.size(); ++i)
00040   {
00041     std::string name, topic, group;
00042     name = static_cast<std::string>(chains[i]["name"]);
00043     group = static_cast<std::string>(chains[i]["planning_group"]);
00044     
00045     if (chains[i].hasMember("topic"))
00046     {
00047       topic = static_cast<std::string>(chains[i]["topic"]);
00048  
00049       boost::shared_ptr<ChainController> controller(new ChainController(name, topic, group));
00050 
00051       for (int j = 0; j < chains[i]["joints"].size(); ++j)
00052       {
00053         controller->joint_names.push_back(static_cast<std::string>(chains[i]["joints"][j]));
00054       }
00055 
00056       ROS_INFO("Waiting for %s...", topic.c_str());
00057       if (!controller->client.waitForServer(ros::Duration(wait_time)))
00058       {
00059         ROS_WARN("Failed to connect to %s", topic.c_str());
00060       }
00061 
00062       if (controller->shouldPlan() && (!move_group_))
00063       {
00064         move_group_.reset(new MoveGroupClient("move_group", true));
00065         if (!move_group_->waitForServer(ros::Duration(wait_time)))
00066         {
00067           ROS_WARN("Failed to connect to move_group");
00068         }
00069       }
00070 
00071       controllers_.push_back(controller);
00072     }
00073   }
00074 
00075   // Parameter to set movement time
00076   nh.param<double>("duration", duration_, 5.0);
00077 
00078   // Parameter to set velocity scaling factor for move_group
00079   nh.param<double>("velocity_factor", velocity_factor_, 1.0);
00080 
00081   subscriber_ = nh.subscribe("/joint_states", 10, &ChainManager::stateCallback, this);
00082 }
00083 
00084 void ChainManager::stateCallback(const sensor_msgs::JointStateConstPtr& msg)
00085 {
00086   if (msg->name.size() != msg->position.size())
00087   {
00088     ROS_ERROR("JointState Error: name array is not same size as position array.");
00089     return;
00090   }
00091 
00092   if (msg->position.size() != msg->velocity.size())
00093   {
00094     ROS_ERROR("JointState Error: position array is not same size as velocity array.");
00095     return;
00096   }
00097 
00098   boost::mutex::scoped_lock lock(state_mutex_);
00099   // Update each joint based on message
00100   for (size_t msg_j = 0; msg_j < msg->name.size(); msg_j++)
00101   {
00102     size_t state_j;
00103     for (state_j = 0; state_j < state_.name.size(); state_j++)
00104     {
00105       if (state_.name[state_j] == msg->name[msg_j])
00106       {
00107         state_.position[state_j] = msg->position[msg_j];
00108         state_.velocity[state_j] = msg->velocity[msg_j];
00109         break;
00110       }
00111     }
00112     if (state_j == state_.name.size())
00113     {
00114       // New joint
00115       state_.name.push_back(msg->name[msg_j]);
00116       state_.position.push_back(msg->position[msg_j]);
00117       state_.velocity.push_back(msg->velocity[msg_j]);
00118     }
00119   }
00120 }
00121 
00122 bool ChainManager::getState(sensor_msgs::JointState* state)
00123 {
00124   boost::mutex::scoped_lock lock(state_mutex_);
00125   *state = state_;
00126   return true;  // TODO: this should actually return whether state is valid
00127 }
00128 
00129 trajectory_msgs::JointTrajectoryPoint
00130 ChainManager::makePoint(const sensor_msgs::JointState& state, const std::vector<std::string>& joints)
00131 {
00132   trajectory_msgs::JointTrajectoryPoint p;
00133   for (size_t i = 0; i < joints.size(); ++i)
00134   {
00135     for (size_t j = 0; j < state.name.size(); ++j)
00136     {
00137       if (joints[i] == state.name[j])
00138       {
00139         p.positions.push_back(state.position[j]);
00140         break;
00141       }
00142     }
00143     p.velocities.push_back(0.0);
00144     p.accelerations.push_back(0.0);
00145     if (p.velocities.size() != p.positions.size())
00146     {
00147       ROS_ERROR_STREAM("Bad move to state, missing " << joints[i]);
00148       exit(-1);
00149     }
00150   }
00151   return p;
00152 }
00153 
00154 bool ChainManager::moveToState(const sensor_msgs::JointState& state)
00155 {
00156   double max_duration = duration_;
00157 
00158   // Split into different controllers
00159   for (size_t i = 0; i < controllers_.size(); ++i)
00160   {
00161     control_msgs::FollowJointTrajectoryGoal goal;
00162     goal.trajectory.joint_names = controllers_[i]->joint_names;
00163 
00164     trajectory_msgs::JointTrajectoryPoint p = makePoint(state, controllers_[i]->joint_names);
00165     if (controllers_[i]->shouldPlan())
00166     {
00167       // Call MoveIt
00168       moveit_msgs::MoveGroupGoal moveit_goal;
00169       moveit_goal.request.group_name = controllers_[i]->chain_planning_group;
00170       moveit_goal.request.num_planning_attempts = 1;
00171       moveit_goal.request.allowed_planning_time = 5.0;
00172 
00173       moveit_msgs::Constraints c1;
00174       c1.joint_constraints.resize(controllers_[i]->joint_names.size());
00175       for (size_t c = 0; c < controllers_[i]->joint_names.size(); c++)
00176       {
00177         c1.joint_constraints[c].joint_name = controllers_[i]->joint_names[c];
00178         c1.joint_constraints[c].position = p.positions[c];
00179         c1.joint_constraints[c].tolerance_above = 0.01;
00180         c1.joint_constraints[c].tolerance_below = 0.01;
00181         c1.joint_constraints[c].weight = 1.0;
00182       }
00183       moveit_goal.request.goal_constraints.push_back(c1);
00184 
00185       // Reduce speed
00186       moveit_goal.request.max_velocity_scaling_factor = velocity_factor_;
00187 
00188       // All diffs
00189       moveit_goal.request.start_state.is_diff = true;
00190       moveit_goal.planning_options.planning_scene_diff.is_diff = true;
00191       moveit_goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
00192 
00193       // Just make the plan, we will execute it
00194       moveit_goal.planning_options.plan_only = true;
00195 
00196       move_group_->sendGoal(moveit_goal);
00197       move_group_->waitForResult();
00198       MoveGroupResultPtr result = move_group_->getResult();
00199       if (result->error_code.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
00200       {
00201         // Unable to plan, return error
00202         return false;
00203       }
00204 
00205       goal.trajectory = result->planned_trajectory.joint_trajectory;
00206       max_duration = std::max(max_duration, goal.trajectory.points[goal.trajectory.points.size()-1].time_from_start.toSec());
00207     }
00208     else
00209     {
00210       // Go directly to point
00211       p.time_from_start = ros::Duration(duration_);
00212       goal.trajectory.points.push_back(p);
00213     }
00214 
00215     goal.goal_time_tolerance = ros::Duration(1.0);
00216 
00217     // Call actions
00218     controllers_[i]->client.sendGoal(goal);
00219   }
00220 
00221   // Wait for results
00222   for (size_t i = 0; i < controllers_.size(); ++i)
00223   {
00224     controllers_[i]->client.waitForResult(ros::Duration(max_duration*1.5));
00225     // TODO: catch errors with clients
00226   }
00227 
00228   return true;
00229 }
00230 
00231 bool ChainManager::waitToSettle()
00232 {
00233   sensor_msgs::JointState state;
00234 
00235   // TODO: timeout?
00236   while (true)
00237   {
00238     getState(&state);
00239     bool settled = true;
00240 
00241     // For each joint in state message
00242     for (size_t j = 0; j < state.name.size(); ++j)
00243     {
00244       // Is this joint even a concern?
00245       if (fabs(state.velocity[j]) < 0.001)
00246         continue;
00247 
00248       for (size_t i = 0; i < controllers_.size(); ++i)
00249       {
00250         for (size_t k = 0; k < controllers_[i]->joint_names.size(); ++k)
00251         {
00252           if (controllers_[i]->joint_names[k] == state.name[j])
00253           {
00254             settled = false;
00255             break;
00256           }
00257         }
00258       }
00259 
00260       // If at least one joint is not settled, break out of this for loop
00261       if (!settled)
00262         break;
00263     }
00264 
00265     // If all joints are settled, break out of while loop
00266     if (settled)
00267       break;
00268 
00269     ros::spinOnce();
00270   }
00271 
00272   return true;
00273 }
00274 
00275 std::vector<std::string> ChainManager::getChains()
00276 {
00277   std::vector<std::string> chains;
00278   for (size_t i = 0; i < controllers_.size(); ++i)
00279   {
00280     chains.push_back(controllers_[i]->chain_name);
00281   }
00282   return chains;
00283 }
00284 
00285 std::vector<std::string> ChainManager::getChainJointNames(
00286   const std::string& chain_name)
00287 {
00288   for (size_t i = 0; i < controllers_.size(); ++i)
00289   {
00290     if (controllers_[i]->chain_name == chain_name)
00291       return controllers_[i]->joint_names;
00292   }
00293   std::vector<std::string> empty;
00294   return empty;
00295 }
00296 
00297 std::string ChainManager::getPlanningGroupName(
00298   const std::string& chain_name)
00299 {
00300   for (size_t i = 0; i < controllers_.size(); ++i)
00301   {
00302     if (controllers_[i]->chain_name == chain_name)
00303       return controllers_[i]->chain_planning_group;
00304   }
00305   std::vector<std::string> empty;
00306   return std::string("");
00307 }
00308 
00309 }  // namespace robot_calibration


robot_calibration
Author(s): Michael Ferguson
autogenerated on Sat May 20 2017 02:35:31