armadillo: armadillo: mpMain.cpp Source File

00001 /*
00002 * Software License Agreement (BSD License) 
00003 *
00004 * Copyright (c) 2011, Yaskawa America, Inc.
00005 * All rights reserved.
00006 *
00007 * Redistribution and use in source and binary forms, with or without
00008 * modification, are permitted provided that the following conditions are met:
00009 *
00010 *       * Redistributions of source code must retain the above copyright
00011 *       notice, this list of conditions and the following disclaimer.
00012 *       * Redistributions in binary form must reproduce the above copyright
00013 *       notice, this list of conditions and the following disclaimer in the
00014 *       documentation and/or other materials provided with the distribution.
00015 *       * Neither the name of the Yaskawa America, Inc., nor the names 
00016 *       of its contributors may be used to endorse or promote products derived
00017 *       from this software without specific prior written permission.
00018 *
00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 * POSSIBILITY OF SUCH DAMAGE.
00030 */ 
00031 
00032 //mpMain.cpp
00033 //
00034 //This contains mpUsrRoot which is the entry point for your MotoPlus application
00035 #include "motoPlus.h"
00036 #include "ros_conversion.h"
00037 #include "mp_default_main.h"
00038 
00039 #include "tcp_server.h"
00040 #include "gripper_handler.h"
00041 #include "trajectory_download_handler.h"
00042 #include "armadillo.h"
00043 #include "message_manager.h"
00044 #include "controller.h"
00045 
00046 
00047 // Global controller
00048 motoman::controller::Controller rbtCtrl;
00049 
00050 
00051 void gripperServer(void)
00052 {
00053 
00054 
00055     using namespace industrial::tcp_server;
00056     using namespace industrial::simple_socket;
00057     using namespace industrial::message_manager;
00058     using namespace industrial::simple_message;
00059     using namespace industrial::armadillo;
00060     using namespace armadillo::gripper_handler;
00061     
00062     TcpServer connection;
00063     GripperHandler gHandler;
00064     MessageManager manager;
00065     
00066     connection.init(StandardSocketPorts::IO);
00067     connection.makeConnect();
00068     
00069     manager.init(&connection);
00070     
00071     gHandler.init(&connection, &rbtCtrl);
00072     manager.add(&gHandler);
00073     manager.spin();
00074  
00075 
00076     
00077 }
00078 
00079 void motionDownloadServer(void)
00080 {
00081 
00082 
00083     using namespace industrial::tcp_server;
00084     using namespace industrial::simple_socket;
00085     using namespace industrial::message_manager;
00086     using namespace industrial::simple_message;
00087     using namespace industrial::armadillo;
00088     using namespace armadillo::trajectory_download_handler;
00089     
00090     TcpServer connection;
00091     TrajectoryDownloadHandler tdHandler;
00092     MessageManager manager;
00093     
00094     connection.init(StandardSocketPorts::MOTION);
00095     connection.makeConnect();
00096     
00097     manager.init(&connection);
00098     
00099     tdHandler.init(&connection, &rbtCtrl);
00100     manager.add(&tdHandler);
00101     manager.spin();
00102  
00103 
00104     
00105 }
00106 
00107 
00108 
00109 
00110 using namespace motoman::mp_default_main;
00111 using namespace motoman::ros_conversion;
00112 
00113 int motion_server_task_ID; \
00114 int system_server_task_ID; \
00115 int state_server_task_ID; \
00116 int gripper_server_task_ID; \
00117 extern "C" void mpUsrRoot(int arg1, int arg2, int arg3, int arg4, int arg5, int arg6, int arg7, int arg8, int arg9, int arg10)
00118 {  
00119   initJointConversion( MotomanRobotModels::SIA_10D );
00120   motion_server_task_ID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (
00121                                         FUNCPTR)motionServer, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); 
00122   //motion_server_task_ID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (
00123   //                                      FUNCPTR)motionDownloadServer, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); 
00124   //system_server_task_ID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)systemServer, 
00125   //                                    arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); 
00126   state_server_task_ID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, 
00127                                     (FUNCPTR)stateServer, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); 
00128   gripper_server_task_ID = mpCreateTask(MP_PRI_TIME_NORMAL, MP_STACK_SIZE, (FUNCPTR)gripperServer, 
00129                                         arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, arg10); 
00130   mpExitUsrRoot; //Ends the initialization task. 
00131 }