robot_state.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan, E. Gil Jones */

#ifndef MOVEIT_ROBOT_STATE_ROBOT_STATE_
#define MOVEIT_ROBOT_STATE_ROBOT_STATE_

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/joint_state_group.h>
#include <moveit/robot_state/attached_body.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>

namespace robot_state
{

typedef boost::function<void(AttachedBody *body, bool attached)> AttachedBodyCallback;

class RobotState
{
  friend class LinkState;
  friend class JointState;
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  RobotState(const robot_model::RobotModelConstPtr &kinematic_model);

  RobotState(const RobotState& state);

  ~RobotState();

  bool setStateValues(const std::vector<double>& joint_state_values);

  void setStateValues(const std::map<std::string, double>& joint_state_map);

  void setStateValues(const std::map<std::string, double>& joint_state_map, std::vector<std::string>& missing);

  void setStateValues(const sensor_msgs::JointState& msg);

  void setStateValues(const std::vector<std::string>& joint_names,
                      const std::vector<double>& joint_values);

  void getStateValues(std::vector<double>& joint_state_values) const;

  void getStateValues(std::map<std::string, double>& joint_state_values) const;

  void getStateValues(sensor_msgs::JointState& msg) const;

  void updateLinkTransforms();

  bool updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false);

  const robot_model::RobotModelConstPtr& getRobotModel() const
  {
    return kinematic_model_;
  }

  unsigned int getVariableCount() const
  {
    return kinematic_model_->getVariableCount();
  }

  void setToDefaultValues();

  void setToRandomValues();

  bool satisfiesBounds(const std::vector<std::string>& joints) const;

  bool satisfiesBounds(const std::string& joint) const;

  bool satisfiesBounds() const;

  void enforceBounds();

  const JointStateGroup* getJointStateGroup(const std::string &name) const;

  JointStateGroup* getJointStateGroup(const std::string &name);

  bool hasJointStateGroup(const std::string &name) const;

  bool hasJointState(const std::string &joint) const;

  bool hasLinkState(const std::string &link) const;

  JointState* getJointState(const std::string &joint) const;

  JointState* getJointState(const robot_model::JointModel *jmodel) const
  {
    // \todo make this more efficient (store index of JointState in the model)
    return getJointState(jmodel->getName());
  }

  LinkState* getLinkState(const std::string &link) const;

  const std::vector<JointState*>& getJointStateVector() const
  {
    return joint_state_vector_;
  }

  const std::vector<LinkState*>& getLinkStateVector() const
  {
    return link_state_vector_;
  }

  const std::map<std::string, JointStateGroup*>& getJointStateGroupMap() const
  {
    return joint_state_group_map_;
  }

  void getJointStateGroupNames(std::vector<std::string>& names) const;

  void attachBody(AttachedBody *attached_body);

  void attachBody(const std::string &id,
                  const std::vector<shapes::ShapeConstPtr> &shapes,
                  const EigenSTL::vector_Affine3d &attach_trans,
                  const std::set<std::string> &touch_links,
                  const std::string &link_name,
                  const sensor_msgs::JointState &detach_posture = sensor_msgs::JointState());
  void attachBody(const std::string &id,
                  const std::vector<shapes::ShapeConstPtr> &shapes,
                  const EigenSTL::vector_Affine3d &attach_trans,
                  const std::vector<std::string> &touch_links,
                  const std::string &link_name,
                  const sensor_msgs::JointState &detach_posture = sensor_msgs::JointState());

  void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies) const;

  bool clearAttachedBody(const std::string &id);

  void clearAttachedBodies(const std::string &link_name);

  void clearAttachedBodies();

  const AttachedBody* getAttachedBody(const std::string &name) const;

  bool hasAttachedBody(const std::string &id) const;

  const Eigen::Affine3d& getFrameTransform(const std::string &id) const;

  void computeAABB(std::vector<double> &aabb) const;

  bool knowsFrameTransform(const std::string &id) const;

  void printStateInfo(std::ostream &out = std::cout) const;

  std::string getStateTreeString(const std::string& prefix = "") const;

  void printTransforms(std::ostream &out = std::cout) const;

  const Eigen::Affine3d& getRootTransform() const;

  void setRootTransform(const Eigen::Affine3d &transform);

  random_numbers::RandomNumberGenerator& getRandomNumberGenerator();

  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       const std_msgs::ColorRGBA& color,
                       const std::string& ns,
                       const ros::Duration& dur,
                       bool include_attached = false) const;

  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       bool include_attached = false) const;

  void interpolate(const RobotState &to, const double t, RobotState &dest) const;

  double infinityNormDistance(const robot_state::RobotState *other) const;

  double distance(const RobotState &state) const;

  RobotState& operator=(const RobotState &other);

  void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback);

private:

  void buildState();
  void copyFrom(const RobotState &ks);
  void printTransform(const std::string &st, const Eigen::Affine3d &t, std::ostream &out = std::cout) const;
  void getStateTreeJointString(std::stringstream& ss, const robot_state::JointState* js, const std::string& prefix, bool last) const;
  static void getPoseString(std::stringstream& ss, const Eigen::Affine3d& mtx, const std::string& pfx = "");


  robot_model::RobotModelConstPtr kinematic_model_;

  std::vector<JointState*>                joint_state_vector_;
  std::map<std::string, JointState*>      joint_state_map_;

  std::vector<LinkState*>                 link_state_vector_;

  std::map<std::string, LinkState*>       link_state_map_;

  Eigen::Affine3d                         root_transform_;

  std::map<std::string, JointStateGroup*> joint_state_group_map_;

  std::map<std::string, AttachedBody*>    attached_body_map_;

  boost::scoped_ptr<random_numbers::RandomNumberGenerator> rng_;

  AttachedBodyCallback attached_body_update_callback_;
};

typedef boost::shared_ptr<RobotState> RobotStatePtr;
typedef boost::shared_ptr<const RobotState> RobotStateConstPtr;

}

#endif