noid_robot_hardware.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Yohei Kakiuchi (JSK lab.)
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Open Source Robotics Foundation
18  * nor the names of its contributors may be
19  * used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /*
37  Author: Yohei Kakiuchi
38 */
39 
40 #ifndef _NOID_ROBOT_HW_H_
41 #define _NOID_ROBOT_HW_H_
42 
43 // ros_control
44 #include <control_toolbox/pid.h>
51 
52 // ROS
53 #include <ros/ros.h>
54 #include <angles/angles.h>
55 
56 // URDF
57 #include <urdf/model.h>
58 
59 // AERO
60 #include "noid_upper_controller.h"
61 #include "noid_lower_controller.h"
62 #include "robot_stroke_converter.h"
63 #include "stroke_mask.h"
64 
65 #include <mutex>
66 
67 using namespace noid;
68 using namespace controller;
69 using namespace common;
70 
72 {
73 
75 {
76 public:
78 
79  virtual ~NoidRobotHW() {}
80 
81  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh);
82  virtual void read(const ros::Time& time, const ros::Duration& period);
83  virtual void write(const ros::Time& time, const ros::Duration& period);
84 
85  void readPos(const ros::Time& time, const ros::Duration& period, bool update);
86  void writeWheel(const std::vector< std::string> &_names, const std::vector<int16_t> &_vel, double _tm_sec);
87  double getPeriod() { return ((double)CONTROL_PERIOD_US_) / (1000 * 1000); }
88 
89  //--specific functions--
90  void runHandScript(uint8_t _number, uint16_t _script, uint8_t _current);
91  void writeWheel(std::vector<int16_t> &_vel);
92  //void startWheelServo();
93  //void stopWheelServo();
94  void onWheelServo(bool _value);
95  //----------------------
96 
97 protected:
98  // Methods used to control a joint.
99  enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
100  enum JointType {NONE, PRISMATIC, ROTATIONAL, CONTINUOUS, FIXED};
101 
102  unsigned int number_of_angles_;
103 
106 
108 
109  std::vector<std::string> joint_list_;
110  std::vector<double> joint_effort_limits_;
111  std::vector<JointType> joint_types_;
112  std::vector<ControlMethod> joint_control_methods_;
113  std::vector<double> joint_position_;
114  std::vector<double> joint_velocity_;
115  std::vector<double> joint_effort_;
116  std::vector<double> joint_position_command_;
117  std::vector<double> joint_velocity_command_;
118  std::vector<double> joint_effort_command_;
119 
120 
121  std::vector<double> prev_ref_positions_;
122  std::vector<int16_t> upper_act_strokes_;
123  std::vector<int16_t> lower_act_strokes_;
124 
127 
130 
134 
135  std::mutex mutex_lower_;
136  std::mutex mutex_upper_;
137 
138  std::vector<std::string> joint_names_upper_;
139  std::vector<std::string> joint_names_lower_;
140  std::string robot_model;
141 };
142 
144 }
145 
146 #endif
boost::shared_ptr< NoidLowerController > controller_lower_
boost::shared_ptr< NoidUpperController > controller_upper_
std::vector< std::string > joint_names_upper_
std::vector< std::string > joint_list_
std::vector< int16_t > lower_act_strokes_
hardware_interface::PositionJointInterface pj_interface_
std::vector< double > prev_ref_positions_
std::vector< JointType > joint_types_
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_
std::vector< double > joint_position_command_
std::vector< double > joint_effort_limits_
boost::shared_ptr< NoidRobotHW > NoidRobotHWPtr
std::vector< ControlMethod > joint_control_methods_
std::vector< double > joint_effort_command_
void update(controller_manager::ControllerManager &cm, const ros::TimerEvent &e)
rl read(is)
hardware_interface::JointStateInterface js_interface_
std::vector< double > joint_velocity_command_
std::vector< std::string > joint_names_lower_
std::vector< int16_t > upper_act_strokes_


noid_ros_controller
Author(s): Yohei Kakiuchi
autogenerated on Sat Jul 20 2019 03:44:30