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
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 #include <cob_utilities/IniFile.h>
00060
00061
00062 #include <ros/ros.h>
00063
00064
00065 #include <sensor_msgs/JointState.h>
00066
00067
00068 #include <cob_srvs/Trigger.h>
00069 #include <cob_base_drive_chain/GetJointState.h>
00070
00071
00072
00073
00074
00075
00076 class NodeClass
00077 {
00078
00079 public:
00080
00081 ros::NodeHandle n;
00082
00083
00084 ros::Publisher topicPub_JointStateCmd;
00085
00086
00087 ros::Subscriber topicSub_demoSubscribe;
00088
00089
00090
00091
00092
00093 ros::ServiceClient srvClient_GetJointState;
00094 ros::ServiceClient srvClient_InitPltf;
00095 ros::ServiceClient srcClient_ShutdownPltf;
00096
00097 double m_dSpeedRadS;
00098 int iNumMotors;
00099
00100
00101 NodeClass()
00102 {
00103 topicPub_JointStateCmd = n.advertise<sensor_msgs::JointState>("base_driver/JointStateCmd", 1);
00104
00105 srvClient_GetJointState = n.serviceClient<cob_base_drive_chain::GetJointState>("base_driver/GetJointState");
00106 srvClient_InitPltf = n.serviceClient<cob_srvs::Trigger>("base_driver/Init");
00107 srcClient_ShutdownPltf = n.serviceClient<cob_srvs::Trigger>("base_driver/Shutdown");
00108
00109
00110
00111 m_dSpeedRadS = 6;
00112 iNumMotors = 8;
00113
00114 }
00115
00116
00117 ~NodeClass()
00118 {
00119 }
00120
00121
00122
00123
00124
00125
00126
00127 int init();
00128
00129 int simpleDriveTest(int argc, char** argv);
00130 };
00131
00132
00133
00134 int main(int argc, char** argv)
00135 {
00136
00137 ros::init(argc, argv, "drive_identification_node");
00138
00139 NodeClass node;
00140 if(node.init() == 1) return 1;
00141 node.simpleDriveTest(argc, argv);
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153 return 0;
00154 }
00155
00156
00157
00158 int NodeClass::init(){
00159
00160 cob_srvs::Trigger data;
00161 srvClient_InitPltf.call(data);
00162
00163 if(data.response.success.data != true) {
00164 ROS_ERROR("Failed to initialize Platform using base_drive_chain_node");
00165 return 1;
00166 } else ROS_INFO("Successfully initialized base_drive_chain_node");
00167
00168
00169 ROS_INFO("Simple_Drive_node init successful");
00170
00171 return 0;
00172 }
00173
00174 int NodeClass::simpleDriveTest(int argc, char** argv) {
00175 double startTime;
00176 sensor_msgs::JointState msgDriveCmd;
00177 cob_base_drive_chain::GetJointState srvGetJointState;
00178 msgDriveCmd.velocity.resize(iNumMotors);
00179
00180 switch (argc){
00181
00182 case 10:
00183 startTime = ros::Time::now().toSec();
00184 while(ros::Time::now().toSec() - startTime < atof(argv[1])) {
00185 for(int i = 0; i<iNumMotors; i++) {
00186 msgDriveCmd.velocity[i] = atof(argv[i+2]);
00187 }
00188 topicPub_JointStateCmd.publish(msgDriveCmd);
00189 srvClient_GetJointState.call(srvGetJointState);
00190
00191 ROS_INFO_STREAM("Actual Vel for Motor " << 0 << " is " << srvGetJointState.response.jointstate.velocity[0]);
00192 }
00193
00194
00195 for(int i = 0; i<iNumMotors; i++) {
00196 msgDriveCmd.velocity[i] = 0;
00197 }
00198 topicPub_JointStateCmd.publish(msgDriveCmd);
00199 break;
00200
00201 default:
00202 ROS_ERROR("No input parameters set. Usage as follows:\nDURATION(s) SPEEDMOT1(radS) SPEEDMOT2 ... SPEEDMOT8");
00203 ROS_ERROR("Your inputs are:");
00204 for(int i=0;i<argc;i++) {
00205 ROS_ERROR_STREAM(i << " " << argv[i]);
00206 }
00207 break;
00208 }
00209
00210
00211
00212 ROS_INFO("Successfully finished simpleDriveTest");
00213
00214 return 0;
00215 }
00216