robot.hpp

Go to the documentation of this file.

/*
 * Copyright 2016 SoftBank Robotics Europe
 *
 * 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.
 *
*/

#ifndef NAOQI_DCM_DRIVER_H
#define NAOQI_DCM_DRIVER_H

// Boost Headers
#include <boost/shared_ptr.hpp>

// NAOqi Headers
#include <qi/session.hpp>
#include <qi/anyobject.hpp>
#include <qi/os.hpp>
#include <qi/anyvalue.hpp>

// ROS Headers
#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float32.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

#include <controller_manager/controller_manager.h>

#include "naoqi_dcm_driver/diagnostics.hpp"
#include "naoqi_dcm_driver/memory.hpp"
#include "naoqi_dcm_driver/dcm.hpp"
#include "naoqi_dcm_driver/motion.hpp"

template<typename T, size_t N>
T * end(T (&ra)[N]) {
    return ra + N;
}

class Robot : public hardware_interface::RobotHW
{
public:
  Robot(qi::SessionPtr session);

  ~Robot();

  void stopService();

  bool isConnected();

  bool connect();

  void run();

private:
  bool initializeControllers(const std::vector <std::string> &joints_names);

  void subscribe();

  bool loadParams();

  void controllerLoop();

  void commandVelocity(const geometry_msgs::TwistConstPtr &msg);

  void publishBaseFootprint(const ros::Time &ts);

  std::vector <bool> checkJoints();

  void readJoints();

  void publishJointStateFromAlMotion();

  void writeJoints();

  bool setStiffness(const float &stiffness);

  void ignoreMimicJoints(std::vector <std::string> *joints);

  boost::scoped_ptr <ros::NodeHandle> nhPtr_;

  boost::shared_ptr <Diagnostics> diagnostics_;

  boost::shared_ptr <Memory> memory_;

  boost::shared_ptr <DCM> dcm_;

  boost::shared_ptr <Motion> motion_;

  ros::Subscriber cmd_moveto_sub_;

  tf::TransformBroadcaster base_footprint_broadcaster_;

  tf::TransformListener base_footprint_listener_;

  ros::Publisher stiffness_pub_;

  ros::Publisher diag_pub_;

  ros::Publisher joint_states_pub_;

  std_msgs::Float32 stiffness_;

  sensor_msgs::JointState joint_states_topic_;

  controller_manager::ControllerManager* manager_;

  std::string session_name_;

  bool is_connected_;

  std::string body_type_;

  std::string prefix_;

  std::string odom_frame_;

  int topic_queue_;

  double high_freq_;

  double controller_freq_;

  double joint_precision_;

  qi::SessionPtr _session;

  std::vector <std::string> motor_groups_;

  hardware_interface::JointStateInterface jnt_state_interface_;

  hardware_interface::PositionJointInterface jnt_pos_interface_;

  std::vector <std::string> qi_joints_;

  std::vector <double> qi_commands_;

  std::vector <std::string> hw_joints_;

  std::vector <bool> hw_enabled_;

  std::vector <double> hw_commands_;

  std::vector <double> hw_angles_;

  std::vector <double> hw_velocities_;

  std::vector <double> hw_efforts_;

  bool use_dcm_;

  float stiffness_value_;
};

#endif // NAOQI_DCM_DRIVER_H