base_module.h
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2017 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /* Authors: SCH, Kayman */
18 
19 #ifndef BASEMODULE_H_
20 #define BASEMODULE_H_
21 
22 #include <map>
23 #include <boost/thread.hpp>
24 #include <yaml-cpp/yaml.h>
25 
26 #include <ros/ros.h>
27 #include <ros/callback_queue.h>
28 #include <ros/package.h>
29 #include <std_msgs/Int16.h>
30 #include <std_msgs/Float64.h>
31 #include <std_msgs/String.h>
32 #include <geometry_msgs/Pose.h>
33 
35 #include "robotis_controller_msgs/JointCtrlModule.h"
36 #include "robotis_controller_msgs/SetModule.h"
37 #include "robotis_controller_msgs/StatusMsg.h"
40 
41 #include "base_module_state.h"
42 
43 namespace robotis_op
44 {
45 
47 {
48  public:
49  double position_;
50  double velocity_;
51  double effort_;
52 
53  int p_gain_;
54  int i_gain_;
55  int d_gain_;
56 
57 };
58 
60 {
61 
62  public:
63  BaseJointData curr_joint_state_[ MAX_JOINT_ID + 1];
64  BaseJointData goal_joint_state_[ MAX_JOINT_ID + 1];
65  BaseJointData fake_joint_state_[ MAX_JOINT_ID + 1];
66 
67 };
68 
70 {
71  public:
72  BaseModule();
73  virtual ~BaseModule();
74 
75  /* ROS Framework Functions */
76  void initialize(const int control_cycle_msec, robotis_framework::Robot *robot);
77  void process(std::map<std::string, robotis_framework::Dynamixel *> dxls, std::map<std::string, double> sensors);
78 
79  void stop();
80  bool isRunning();
81 
82  void onModuleEnable();
83  void onModuleDisable();
84 
85  /* ROS Topic Callback Functions */
86  void initPoseMsgCallback(const std_msgs::String::ConstPtr& msg);
87 
88  /* ROS Calculation Functions */
89  void initPoseTrajGenerateProc();
90 
91  void poseGenerateProc(Eigen::MatrixXd joint_angle_pose);
92  void poseGenerateProc(std::map<std::string, double>& joint_angle_pose);
93 
94  /* Parameter */
97 
98  private:
99  void queueThread();
100  void setCtrlModule(std::string module);
101  void callServiceSettingModule(const std::string &module_name);
102  void parseInitPoseData(const std::string &path);
103  void publishStatusMsg(unsigned int type, std::string msg);
104 
106  boost::thread queue_thread_;
107  boost::thread tra_gene_tread_;
108 
111 
113 
114  std::map<std::string, int> joint_name_to_id_;
115 
118 };
119 
120 }
121 
122 #endif /* BASEMODULE_H_ */
BaseJointState * joint_state_
Definition: base_module.h:96
ROSCONSOLE_DECL void initialize()
ros::Publisher status_msg_pub_
Definition: base_module.h:109
boost::thread queue_thread_
Definition: base_module.h:106
ros::ServiceClient set_module_client_
Definition: base_module.h:112
BaseModuleState * base_module_state_
Definition: base_module.h:95
std::map< std::string, int > joint_name_to_id_
Definition: base_module.h:114
ros::Publisher set_ctrl_module_pub_
Definition: base_module.h:110
#define MAX_JOINT_ID
boost::thread tra_gene_tread_
Definition: base_module.h:107


op3_base_module
Author(s): Kayman , SCH
autogenerated on Mon Jun 10 2019 14:41:14