30 #include <boost/algorithm/string.hpp> 31 #include <boost/foreach.hpp> 32 #include <boost/thread.hpp> 34 #include "bcap_core/dn_common.h" 37 #include <std_msgs/Int32.h> 38 #include <std_msgs/Float32.h> 39 #include <std_msgs/Float64.h> 40 #include <std_msgs/String.h> 41 #include <std_msgs/Bool.h> 42 #include <std_msgs/Float32MultiArray.h> 43 #include <std_msgs/Float64MultiArray.h> 44 #include "denso_robot_core/Joints.h" 45 #include "denso_robot_core/ExJoints.h" 46 #include "denso_robot_core/PoseData.h" 47 #include "denso_robot_core/MoveStringAction.h" 48 #include "denso_robot_core/MoveValueAction.h" 49 #include "denso_robot_core/DriveStringAction.h" 50 #include "denso_robot_core/DriveValueAction.h" 56 #define MESSAGE_QUEUE (100) 57 #define DEFAULT_TIMEOUT (0.0) 92 for(
int i = 0; i <pose.size(); i++) {
94 if(i != pose.size() - 1) {
97 std::cout << std::endl;
110 void ReadLine(std::string &str, std::string erase =
"")
113 std::cin.getline(cline,
sizeof(cline));
114 std::getline(std::cin, str);
115 std::cin.putback(
'\n');
117 if(!erase.compare(
"")) {
119 while((c = str.find_first_of(erase)) != std::string::npos) {
132 std::vector<std::string> vecStr;
134 Int32 ival; Float32 fval; Float64 dval;
135 String sval; Bool bval;
136 Float32MultiArray faryval;
137 Float64MultiArray daryval;
139 std::cout <<
"Please input variable type: ";
142 std::cout <<
"Please input value: ";
144 boost::split(vecStr, strTmp, boost::is_any_of(
","));
150 ival.data = atoi(vecStr.front().c_str());
157 fval.data = atof(vecStr.front().c_str());
164 dval.data = atof(vecStr.front().c_str());
171 sval.data = vecStr.front();
178 bval.data = atoi(vecStr.front().c_str());
185 BOOST_FOREACH(std::string str, vecStr) {
186 faryval.data.push_back(atof(str.c_str()));
194 BOOST_FOREACH(std::string str, vecStr) {
195 daryval.data.push_back(atof(str.c_str()));
210 std::cout <<
"Please input value/number: ";
211 std::cin >> ival.data;
225 std::cout <<
"Please input value/number: ";
226 std::cin >> fltval.data;
247 MoveStringGoal mvStrGoal;
248 MoveStringResultConstPtr mvStrRes;
250 std::cout <<
"Please input interpolation (PTP:=1, CP:=2): ";
251 std::cin >> mvStrGoal.comp;
252 std::cout <<
"Please input pose (ex.: J(0,0,0,0,0,0,0,0)): ";
254 std::cout <<
"Please input option (If skip, input _): ";
256 if(!mvStrGoal.option.compare(
"_")) mvStrGoal.option =
"";
260 acMvStr.sendGoal(mvStrGoal);
262 acMvStr.cancelGoal();
269 gs = acMvStr.getState();
274 mvStrRes = acMvStr.getResult();
276 ROS_INFO(
"State: %s, Result: %X", gs.
toString().c_str(), mvStrRes->HRESULT);
290 std::vector<std::string> vecStr;
296 MoveValueGoal mvValGoal;
297 MoveValueResultConstPtr mvValRes;
299 std::cout <<
"Please input interpolation (PTP:=1, CP:=2): ";
300 std::cin >> mvValGoal.comp;
301 std::cout <<
"Please input pose (ex.: 0,0,0,0,0,0,0,0): ";
303 boost::split(vecStr, strTmp, boost::is_any_of(
","));
304 BOOST_FOREACH(std::string str, vecStr) {
305 mvValGoal.pose.value.push_back(atof(str.c_str()));
307 std::cout <<
"Please input type (P: 0, T: 1, J: 2, V: 3): ";
308 std::cin >> mvValGoal.pose.type;
309 std::cout <<
"Please input pass (@P: -1, @E: -2, @0: 0, @n: n): ";
310 std::cin >> mvValGoal.pose.pass;
311 std::cout <<
"Please input exjoints mode (EX: 1, EXA: 2, No exjoints: 0): ";
312 std::cin >> mvValGoal.pose.exjoints.mode;
313 if(mvValGoal.pose.exjoints.mode != 0) {
315 std::cout <<
"Please input exjoint value (ex.: 7,30.5, If skip input _): ";
317 if(!strTmp.compare(
"_"))
break;
318 boost::split(vecStr, strTmp, boost::is_any_of(
","));
320 jnt.joint = atoi(vecStr.at(0).c_str());
321 jnt.value = atof(vecStr.at(1).c_str());
322 mvValGoal.pose.exjoints.joints.push_back(jnt);
325 std::cout <<
"Please input option (If skip, input _): ";
327 if(!mvValGoal.option.compare(
"_")) mvValGoal.option =
"";
331 acMvVal.sendGoal(mvValGoal);
333 acMvVal.cancelGoal();
340 gs = acMvVal.getState();
345 mvValRes = acMvVal.getResult();
347 ROS_INFO(
"State: %s, Result: %X", gs.
toString().c_str(), mvValRes->HRESULT);
364 DriveStringGoal drvStrGoal;
365 DriveStringResultConstPtr drvStrRes;
367 std::cout <<
"Please input pose (ex.: @0 (1, 10), (2, 20)): ";
369 std::cout <<
"Please input option (If skip, input _): ";
371 if(!drvStrGoal.option.compare(
"_")) drvStrGoal.option =
"";
375 acDrvVal.sendGoal(drvStrGoal);
377 acDrvVal.cancelGoal();
384 gs = acDrvVal.getState();
389 drvStrRes = acDrvVal.getResult();
391 ROS_INFO(
"State: %s, Result: %X", gs.
toString().c_str(), drvStrRes->HRESULT);
405 std::vector<std::string> vecStr;
411 DriveValueGoal drvValGoal;
412 DriveValueResultConstPtr drvValRes;
414 std::cout <<
"Please input pass (@P: -1, @E: -2, @0: 0, @n: n): ";
415 std::cin >> drvValGoal.pass;
417 std::cout <<
"Please input joint value (ex.: 7,30.5, If skip input _): ";
419 if(!strTmp.compare(
"_"))
break;
420 boost::split(vecStr, strTmp, boost::is_any_of(
","));
422 jnt.joint = atoi(vecStr.at(0).c_str());
423 jnt.value = atof(vecStr.at(1).c_str());
424 drvValGoal.pose.push_back(jnt);
426 std::cout <<
"Please input option (If skip, input _): ";
428 if(!drvValGoal.option.compare(
"_")) drvValGoal.option =
"";
432 acDrvVal.sendGoal(drvValGoal);
434 acDrvVal.cancelGoal();
441 gs = acDrvVal.getState();
446 drvValRes = acDrvVal.getResult();
448 ROS_INFO(
"State: %s, Result: %X", gs.
toString().c_str(), drvValRes->HRESULT);
451 int main(
int argc,
char** argv)
454 std::cout <<
"Usage: denso_robot_core mode topic [timeout]" << std::endl;
455 std::cout <<
"mode: 1:= Write variable." << std::endl;
456 std::cout <<
" 2:= Write ID." << std::endl;
457 std::cout <<
" 3:= Change arm group." << std::endl;
458 std::cout <<
" 4:= Move robot with string." << std::endl;
459 std::cout <<
" 5:= Move robot with value." << std::endl;
460 std::cout <<
" 6:= Drive robot with string." << std::endl;
461 std::cout <<
" 7:= Drive robot with value." << std::endl;
462 std::cout <<
" 8:= Change robot speed." << std::endl;
463 std::cout <<
" 9:= Change tool or work number." << std::endl;
467 ros::init(argc, argv,
"denso_robot_core_test");
469 int mode = atoi(argv[1]);
473 timeout = atof(argv[3]);
476 std::cin.putback(
'\n');
void Callback_DriveValueFeedback(const DriveValueActionFeedbackConstPtr &msg)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
void DriveString(const std::string &name, float timeout)
void ReadLine(std::string &str, std::string erase="")
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void Connected(const ros::SingleSubscriberPublisher &pub)
std::string toString() const
void Callback_MoveStringFeedback(const MoveStringActionFeedbackConstPtr &msg)
void WriteVariable(const std::string &name)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void WriteInt32(const std::string &name)
void Callback_DriveStringFeedback(const DriveStringActionFeedbackConstPtr &msg)
void Callback_MoveValueFeedback(const MoveValueActionFeedbackConstPtr &msg)
void PrintFeedback(const std::vector< double > &pose)
int main(int argc, char **argv)
void MoveValue(const std::string &name, float timeout)
void WriteFloat32(const std::string &name)
void MoveString(const std::string &name, float timeout)
void DriveValue(const std::string &name, float timeout)
ROSCPP_DECL void spinOnce()