chain_manager.h

Go to the documentation of this file.

/*
 * Copyright (C) 2014-2015 Fetch Robotics Inc.
 * Copyright (C) 2013-2014 Unbounded Robotics Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
// Author: Michael Ferguson
 
#ifndef ROBOT_CALIBRATION_CAPTURE_CHAIN_MANAGER_H
#define ROBOT_CALIBRATION_CAPTURE_CHAIN_MANAGER_H
 
#include <ros/ros.h>
#include <boost/thread/mutex.hpp>
#include <sensor_msgs/JointState.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <moveit_msgs/MoveGroupAction.h>
 
namespace robot_calibration
{
 
class ChainManager
{
  typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajectoryClient;
  typedef actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> MoveGroupClient;
  typedef boost::shared_ptr<MoveGroupClient> MoveGroupClientPtr;
  typedef boost::shared_ptr<const moveit_msgs::MoveGroupResult> MoveGroupResultPtr;
 
  // This controls a single chain
  struct ChainController
  {
    ChainController(const std::string& name,
                    const std::string& topic,
                    const std::string& planning_group) :
      client(topic, true),
      chain_name(name),
      chain_planning_group(planning_group)
    {
    }
 
    bool shouldPlan()
    {
      return (chain_planning_group != "");
    }
 
    TrajectoryClient client;
    std::string chain_name;
    std::string chain_planning_group;
    std::vector<std::string> joint_names;
  };
 
public:
  ChainManager(ros::NodeHandle& nh, double wait_time = 15.0);
 
  bool moveToState(const sensor_msgs::JointState& state);
 
  bool waitToSettle();
 
  bool getState(sensor_msgs::JointState* state);
 
  std::vector<std::string> getChains();
 
  std::vector<std::string> getChainJointNames(const std::string& chain_name);
 
  // Mainly for testing
  std::string getPlanningGroupName(const std::string& chain_name);
 
private:
  void stateCallback(const sensor_msgs::JointStateConstPtr& msg);
 
  trajectory_msgs::JointTrajectoryPoint makePoint(const sensor_msgs::JointState& state,
                                                  const std::vector<std::string>& joints);
 
  // Subscriber for joint_states topic, storage of message
  ros::Subscriber subscriber_;
  boost::mutex state_mutex_;
  sensor_msgs::JointState state_;
  bool state_is_valid_;
 
  // Mechanisms for passing commands to controllers
  double duration_;
  std::vector<boost::shared_ptr<ChainController> > controllers_;
  MoveGroupClientPtr move_group_;
  double velocity_factor_;  // scaling factor to slow down move_group plans
};
 
}  // namespace robot_calibration
 
#endif  // ROBOT_CALIBRATION_CAPTURE_CHAIN_MANAGER_H