Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include "controller.h"
00033 #include "mp_default_main.h"
00034 #include "log_wrapper.h"
00035 #include "tcp_server.h"
00036 #include "message_manager.h"
00037 #include "input_handler.h"
00038 #include "joint_motion_handler.h"
00039 #include "joint_data.h"
00040 #include "joint_message.h"
00041 #include "robot_status.h"
00042 #include "robot_status_message.h"
00043 #include "simple_message.h"
00044 #include "ros_conversion.h"
00045
00046 #include "motoPlus.h"
00047
00048
00049 namespace motoman
00050 {
00051 namespace mp_default_main
00052 {
00053
00054 void motionServer(void)
00055
00056 {
00057
00058 using namespace industrial::simple_socket;
00059 using namespace industrial::tcp_server;
00060 using namespace industrial::message_manager;
00061 using namespace industrial::simple_message;
00062 using namespace motoman::joint_motion_handler;
00063
00064 TcpServer connection;
00065 JointMotionHandler jmHandler;
00066
00067 MessageManager manager;
00068
00069 connection.init(StandardSocketPorts::MOTION);
00070
00071
00072 connection.makeConnect();
00073
00074 manager.init(&connection);
00075
00076 jmHandler.init(StandardMsgTypes::JOINT, &connection);
00077 manager.add(&jmHandler);
00078 manager.spin();
00079
00080
00081 }
00082
00083
00084 void systemServer(void)
00085
00086 {
00087
00088 using namespace industrial::simple_socket;
00089 using namespace industrial::tcp_server;
00090 using namespace industrial::message_manager;
00091
00092 TcpServer connection;
00093 MessageManager manager;
00094
00095 connection.init(StandardSocketPorts::SYSTEM);
00096 connection.makeConnect();
00097
00098 manager.init(&connection);
00099 manager.spin();
00100
00101 }
00102
00103
00104
00105 void stateServer(void)
00106 {
00107 using motoman::controller::Controller;
00108 using namespace industrial::simple_socket;
00109 using namespace industrial::tcp_server;
00110 using namespace industrial::joint_message;
00111 using namespace industrial::joint_data;
00112 using namespace industrial::robot_status;
00113 using namespace industrial::robot_status_message;
00114 using namespace industrial::simple_message;
00115 using namespace motoman::ros_conversion;
00116
00117
00118 TcpServer connection;
00119
00120 JointData rosJoints;
00121 JointMessage msg;
00122
00123 RobotStatus status;
00124 RobotStatusMessage statusMsg;
00125
00126 SimpleMessage simpMsg;
00127 Controller controller;
00128 float mpJoints[MAX_PULSE_AXES];
00129
00130 const int period = 100;
00131
00132 connection.init(StandardSocketPorts::STATE);
00133
00134 FOREVER
00135 {
00136 connection.makeConnect();
00137
00138 while(connection.isConnected())
00139 {
00140 controller.getActJointPos(mpJoints);
00141 toRosJoint(mpJoints, rosJoints);
00142 msg.init(0, rosJoints);
00143 msg.toTopic(simpMsg);
00144 connection.sendMsg(simpMsg);
00145
00146 controller.getStatus(status);
00147 statusMsg.init(status);
00148 statusMsg.toTopic(simpMsg);
00149 connection.sendMsg(simpMsg);
00150
00151 mpTaskDelay(period);
00152
00153
00154 }
00155
00156
00157 }
00158
00159 }
00160
00161
00162
00163 void ioServer(void)
00164 {
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188 }
00189
00190
00191 }
00192 }