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 #include "Controller.h"
00022 #include <actionlib/server/simple_action_server.h>
00023 #include <tug_ist_diagnosis_msgs/DiagnosisRepairAction.h>
00024 #include <tug_ist_diagnosis_msgs/BoardAction.h>
00025 #include <string>
00026 #include <stdlib.h>
00027
00028 using namespace std;
00029
00030 typedef actionlib::SimpleActionServer<tug_ist_diagnosis_msgs::BoardAction> boardServer;
00031 typedef actionlib::SimpleActionServer<tug_ist_diagnosis_msgs::DiagnosisRepairAction> switchServer;
00032
00033 Controller *contl;
00034
00035
00036 void power_up(const tug_ist_diagnosis_msgs::DiagnosisRepairGoalConstPtr& goal, switchServer* as)
00037 {
00038
00039 tug_ist_diagnosis_msgs::DiagnosisRepairResult result_;
00040 char chnl = contl->get_chnl_from_map(goal->parameter[0].c_str());
00041 ROS_INFO("Request Call for Power Up for dev %s on Channel %d is received.",goal->parameter[0].c_str(),chnl);
00042 if(chnl!=-1)
00043 {
00044 char status;
00045 status = 1;
00046 contl->CallMessageChannelOnOff(chnl,status);
00047 sleep(3);
00048 result_.result = 255;
00049 as->setSucceeded(result_);
00050 ROS_INFO("Request Done for Power Up for dev %s on Channel %d.",goal->parameter[0].c_str(),chnl);
00051 }else
00052 ROS_INFO("Can not Power Up %s.",goal->parameter[0].c_str());
00053 }
00054
00055
00056 void shut_down(const tug_ist_diagnosis_msgs::DiagnosisRepairGoalConstPtr& goal, switchServer* as)
00057 {
00058 tug_ist_diagnosis_msgs::DiagnosisRepairResult result_;
00059 char chnl = contl->get_chnl_from_map(goal->parameter[0].c_str());
00060 ROS_INFO("Request Call for Shut down for dev %s on Channel %c is received.",goal->parameter[0].c_str(),chnl);
00061 if(chnl!=-1)
00062 {
00063 char status;
00064 string str_goal = goal->parameter[0].c_str();
00065 chnl = str_goal[7];
00066 status = 0;
00067 contl->CallMessageChannelOnOff(chnl,status);
00068 sleep(3);
00069 result_.result = 255;
00070 as->setSucceeded(result_);
00071 ROS_INFO("Request Done for Shut down for dev %s on Channel %c.",goal->parameter[0].c_str(),chnl);
00072 }else
00073 ROS_INFO("Can not Shut down Channel %s.",goal->parameter[0].c_str());
00074
00075 }
00076
00077
00078
00079 void execute(const tug_ist_diagnosis_msgs::BoardGoalConstPtr& goal, boardServer* as)
00080 {
00081
00082 int i;
00083 i = goal->command;
00084 if(i==3)
00085 contl->CallMessageRequest();
00086 else if(i==2)
00087 {
00088 char frq;
00089 frq = goal->arg1;
00090 contl->CallMessageBroadCasting(frq);
00091 }
00092 else if(i==4)
00093 {
00094 char chnl;
00095 char status;
00096 chnl = goal->arg1;
00097 status = goal->arg2;
00098 contl->CallMessageChannelOnOff(chnl,status);
00099
00100 }
00101 printf("Otherthan3");
00102 as->setSucceeded();
00103 }
00104
00105
00106
00107
00108 int main( int argc, char **argv)
00109 {
00110 ros::init(argc, argv,"board_controller");
00111 ros::NodeHandle n("~");
00112 int port;
00113 string ip;
00114 n.param<int>("port", port, 5000);
00115 n.param<string>("ip", ip, "127.0.0.1");
00116 contl = new Controller(1,ip,port);
00117
00118 boardServer bserver(n, "board_server", boost::bind(&execute, _1, &bserver), false);
00119 switchServer pserver(n, "power_up", boost::bind(&power_up, _1, &pserver), false);
00120 switchServer sserver(n, "shutdown", boost::bind(&shut_down, _1, &sserver), false);
00121
00122 bserver.start();
00123 pserver.start();
00124 sserver.start();
00125
00126 contl->initController();
00127 ROS_INFO("Board Connected.....");
00128 ros::spin();
00129
00130 }