27 #define BCAP_ROBOT_EXECUTE_ARGS (3) 28 #define BCAP_ROBOT_HALT_ARGS (2) 29 #define BCAP_ROBOT_MOVE_ARGS (4) 30 #define BCAP_ROBOT_SPEED_ARGS (3) 31 #define BCAP_ROBOT_CHANGE_ARGS (2) 33 #define NAME_MOVESTRING "_MoveString" 34 #define NAME_MOVEVALUE "_MoveValue" 35 #define NAME_DRIVEEXSTRING "_DriveExString" 36 #define NAME_DRIVEEXVALUE "_DriveExValue" 37 #define NAME_DRIVEAEXSTRING "_DriveAExString" 38 #define NAME_DRIVEAEXVALUE "_DriveAExValue" 39 #define NAME_SPEED "_Speed" 40 #define NAME_CHANGETOOL "_ChangeTool" 41 #define NAME_CHANGEWORK "_ChangeWork" 64 const std::string& name,
const int* mode)
65 :
DensoRobot(parent, service, handle, name, mode),
66 m_curAct(
ACT_RESET), m_memTimeout(0), m_memRetry(0),
67 m_tsfmt(0), m_timestamp(0),
68 m_sendfmt(0), m_send_miniio(0), m_send_handio(0),
69 m_recvfmt(0), m_recv_miniio(0), m_recv_handio(0),
70 m_send_userio_offset(UserIO::MIN_USERIO_OFFSET),
71 m_send_userio_size(1),
72 m_recv_userio_offset(UserIO::MIN_USERIO_OFFSET),
105 m_actMoveString = boost::make_shared<SimpleActionServer<MoveStringAction> >
109 m_actMoveString->registerPreemptCallback(
112 m_actMoveString->start();
114 m_actMoveValue = boost::make_shared<SimpleActionServer<MoveValueAction> >
192 boost::mutex::scoped_lock lockSrv(
m_mtxSrv);
232 vntArgs.push_back(*vntTmp.get());
261 vntArgs.push_back(*vntTmp.get());
268 int comp,
const VARIANT_Ptr& pose,
const std::string& option)
302 vntArgs.push_back(*vntTmp.get());
314 const std::string& name,
const VARIANT_Ptr& option)
344 vntArgs.push_back(*vntTmp.get());
382 vntTmp->fltVal = value;
386 vntArgs.push_back(*vntTmp.get());
424 vntArgs.push_back(*vntTmp.get());
457 vntArgs.push_back(*vntTmp.get());
488 vntArgs.push_back(*vntTmp.get());
499 num = vntRet->parray->rgsabound->cElements;
501 pose.resize(num - 1);
502 std::copy(&pdblval[1], &pdblval[num], pose.begin());
510 const std::vector<double>& pose, std::vector<double>& joint)
542 vntArgs.push_back(*vntTmp.get());
623 vntTmp->lVal = format;
635 vntArgs.push_back(*vntTmp.get());
642 const std::vector<double>& pose,
VARIANT_Ptr& send,
643 const int miniio,
const int handio,
644 const int recv_userio_offset,
const int recv_userio_size,
645 const int send_userio_offset,
const int send_userio_size,
646 const std::vector<uint8_t>& send_userio)
666 if(joints < pose.size()) {
671 bool send_hio, send_mio, send_uio, recv_uio;
677 if(send_userio_size < send_userio.size()) {
686 int num = 1 + send_hio + send_mio + 3 * send_uio + 2 * recv_uio;
697 memset(pdbl, 0, joints *
sizeof(
double));
698 std::copy(pose.begin(), pose.end(), pdbl);
713 memset(pdbl, 0, joints *
sizeof(
double));
714 std::copy(pose.begin(), pose.end(), pdbl);
723 pvnt[offset].
lVal = miniio;
731 pvnt[offset+0].
lVal = send_userio_offset;
734 pvnt[offset+1].
lVal = send_userio_size * UserIO::USERIO_ALIGNMENT;
739 memset(pbool, 0, send_userio_size *
sizeof(
uint8_t));
740 std::copy(send_userio.begin(), send_userio.end(), pbool);
749 pvnt[offset+0].
lVal = recv_userio_offset;
752 pvnt[offset+1].
lVal = recv_userio_size * UserIO::USERIO_ALIGNMENT;
760 pvnt[offset].
lVal = handio;
773 std::vector<double>& position, std::vector<double>& joint, std::vector<double>& trans,
774 int& miniio,
int& handio,
int& timestamp,
775 std::vector<uint8_t>& recv_userio,
776 std::vector<double>& current)
781 int j = 0, j1 = 0, j2 = 0, joints = 0;
782 std::vector<double> *pose1 = NULL, *pose2 = NULL;
816 bool recv_ts, recv_hio, recv_mio, recv_uio, recv_crt;
824 int num = 1 + recv_ts + recv_hio + recv_mio + recv_uio + recv_crt;
832 if(joints != recv->parray->rgsabound->cElements) {
839 std::copy(pdbl, &pdbl[j1], pose1->begin());
842 std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
847 if(num != recv->parray->rgsabound->cElements) {
857 if(pvnt[offset].vt !=
VT_I4) {
862 timestamp = pvnt[offset].
lVal;
870 || (joints != pvnt[offset].parray->rgsabound->cElements))
878 std::copy(pdbl, &pdbl[j1], pose1->begin());
881 std::copy(&pdbl[j1], &pdbl[joints], pose2->begin());
890 if(pvnt[offset].vt !=
VT_I4) {
895 miniio = pvnt[offset].
lVal;
909 recv_userio.resize(pvnt[offset].parray->rgsabound->cElements);
910 std::copy(pbool, &pbool[pvnt[offset].parray->rgsabound->cElements], recv_userio.begin());
918 if(pvnt[offset].vt !=
VT_I4) {
923 handio = pvnt[offset].
lVal;
931 || (8 != pvnt[offset].parray->rgsabound->cElements))
939 std::copy(pdbl, &pdbl[8], current.begin());
957 MoveStringResult res;
960 boost::mutex::scoped_lock lockAct(
m_mtxAct);
976 hr =
ExecMove(goal->comp, vntPose, goal->option);
999 boost::mutex::scoped_lock lockAct(
m_mtxAct);
1014 hr =
ExecMove(goal->comp, vntPose, goal->option);
1032 const std::string& name,
const DriveStringGoalConstPtr& goal)
1035 DriveStringResult res;
1041 if(!name.compare(
"DriveEx")) {
1045 else if(!name.compare(
"DriveAEx")) {
1052 boost::mutex::scoped_lock lockAct(
m_mtxAct);
1056 actSvr->setAborted(res);
1079 actSvr->setSucceeded(res);
1082 actSvr->setAborted(res);
1090 const std::string& name,
const DriveValueGoalConstPtr& goal)
1093 DriveValueResult res;
1099 if(!name.compare(
"DriveEx")) {
1103 else if(!name.compare(
"DriveAEx")) {
1110 boost::mutex::scoped_lock lockAct(
m_mtxAct);
1114 actSvr->setAborted(res);
1134 for(
int i = 0; i < goal->pose.size(); i++) {
1136 pd.value.push_back(goal->pose.at(i).joint);
1137 pd.value.push_back(goal->pose.at(i).value);
1139 pd.pass = (i == 0) ? goal->pass : 0;
1157 actSvr->setSucceeded(res);
1160 actSvr->setAborted(res);
1173 const std::string& name,
const Int32::ConstPtr& msg)
1175 std::stringstream ss;
1176 ss << name << msg->data;
1182 boost::mutex::scoped_lock lockAct(
m_mtxAct);
1214 boost::mutex::scoped_lock lockAct(
m_mtxAct);
1218 std::vector<double> pose;
1220 MoveStringFeedback fbMvStr;
1221 MoveValueFeedback fbMvVal;
1222 DriveStringFeedback fbDrvStr;
1223 DriveValueFeedback fbDrvVal;
1230 fbMvStr.pose = pose;
1234 fbMvVal.pose = pose;
1238 fbDrvStr.pose = pose;
1242 fbDrvVal.pose = pose;
1246 fbDrvStr.pose = pose;
1250 fbDrvVal.pose = pose;
1275 ROS_WARN(
"Failed to put_SendFormat.");
1291 switch(format & ~RECVFMT_POSE) {
1318 ROS_WARN(
"Failed to put_RecvFormat.");
1324 ROS_WARN(
"Failed to put_RecvFormat.");
1342 ROS_WARN(
"Failed to put_TimeFormat.");
1368 if(value.offset < UserIO::MIN_USERIO_OFFSET)
1370 ROS_WARN(
"User I/O offset has to be greater than %d.",
1371 UserIO::MIN_USERIO_OFFSET - 1);
1375 if(value.offset % UserIO::USERIO_ALIGNMENT)
1377 ROS_WARN(
"User I/O offset has to be multiple of %d.",
1378 UserIO::USERIO_ALIGNMENT);
1384 ROS_WARN(
"User I/O size has to be greater than 0.");
1388 if(value.size < value.value.size())
1390 ROS_WARN(
"User I/O size has to be equal or greater than the value length.");
1401 if(value.offset < UserIO::MIN_USERIO_OFFSET)
1403 ROS_WARN(
"User I/O offset has to be greater than %d.",
1404 UserIO::MIN_USERIO_OFFSET - 1);
1408 if(value.offset % UserIO::USERIO_ALIGNMENT)
1410 ROS_WARN(
"User I/O offset has to be multiple of %d.",
1411 UserIO::USERIO_ALIGNMENT);
1417 ROS_WARN(
"User I/O size has to be greater than 0.");
void get_RecvUserIO(UserIO &value) const
HRESULT SafeArrayUnaccessData(SAFEARRAY *psa)
#define BCAP_ROBOT_CHANGE_ARGS
void put_RecvFormat(int format)
void put_MiniIO(int value)
void Callback_Change(const std::string &name, const Int32::ConstPtr &msg)
SAFEARRAY * SafeArrayCreateVector(uint16_t vt, int32_t lLbound, uint32_t cElements)
HRESULT ParseRecvParameter(const VARIANT_Ptr &recv, std::vector< double > &position, std::vector< double > &joint, std::vector< double > &trans, int &miniio, int &handio, int ×tamp, std::vector< uint8_t > &recv_userio, std::vector< double > ¤t)
HRESULT StartService(ros::NodeHandle &node)
void Callback_DriveString(const std::string &name, const DriveStringGoalConstPtr &goal)
void put_RecvUserIO(const UserIO &value)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::vector< uint8_t > m_recv_userio
void Callback_Speed(const Float32::ConstPtr &msg)
std::vector< VARIANT, VariantAllocator< VARIANT > > VARIANT_Vec
std::vector< uint32_t > Handle_Vec
boost::shared_ptr< SimpleActionServer< DriveValueAction > > m_actDriveExValue
HRESULT ExecSlaveMode(const std::string &name, int32_t format, int32_t option=0)
int get_RecvFormat() const
std::vector< uint8_t > m_send_userio
ros::Subscriber m_subChangeTool
std::vector< BCAPService_Ptr > Service_Vec
void VariantInit(VARIANT *pvarg)
void Callback_MoveString(const MoveStringGoalConstPtr &goal)
#define NAME_DRIVEAEXSTRING
ros::Subscriber m_subSpeed
std::vector< double > m_position
#define NAME_DRIVEEXVALUE
void put_TimeFormat(int format)
HRESULT ExecChange(const std::string &value)
int get_TimeFormat() const
HRESULT ExecSlaveMove(const std::vector< double > &pose, std::vector< double > &joint)
BSTR SysAllocString(const wchar_t *sz)
std::vector< double > m_trans
std::vector< double > m_current
_DN_EXP_COMMON HRESULT VariantCopy(VARIANT *pvargDest, const VARIANT *pvargSrc)
HRESULT ExecCurJnt(std::vector< double > &pose)
#define BCAP_ROBOT_SPEED_ARGS
boost::interprocess::unique_ptr< VARIANT, variant_deleter > VARIANT_Ptr
HRESULT SafeArrayAccessData(SAFEARRAY *psa, void **ppvData)
static BSTR ConvertStringToBSTR(const std::string &str)
HRESULT ExecSpeed(float value)
boost::shared_ptr< SimpleActionServer< DriveStringAction > > m_actDriveExString
void Callback_MoveValue(const MoveValueGoalConstPtr &goal)
HRESULT ChangeMode(int mode)
int get_SendFormat() const
boost::shared_ptr< SimpleActionServer< DriveStringAction > > m_actDriveAExString
HRESULT ExecDrive(const std::string &name, const VARIANT_Ptr &option)
void get_Current(std::vector< double > ¤t) const
#define BCAP_ROBOT_MOVE_ARGS
HRESULT ExecMove(int comp, const VARIANT_Ptr &pose, const std::string &option)
HRESULT CreateSendParameter(const std::vector< double > &pose, VARIANT_Ptr &send, const int miniio=0, const int handio=0, const int recv_userio_offset=0, const int recv_userio_size=0, const int send_userio_offset=0, const int send_userio_size=0, const std::vector< uint8_t > &send_userio=std::vector< uint8_t >())
#define BCAP_ROBOT_EXECUTE_ARGS
ros::Subscriber m_subChangeWork
boost::shared_ptr< SimpleActionServer< MoveValueAction > > m_actMoveValue
DensoRobotRC8(DensoBase *parent, Service_Vec &service, Handle_Vec &handle, const std::string &name, const int *mode)
virtual HRESULT StartService(ros::NodeHandle &node)
boost::shared_ptr< SimpleActionServer< MoveStringAction > > m_actMoveString
boost::shared_ptr< SimpleActionServer< DriveValueAction > > m_actDriveAExValue
virtual HRESULT StopService()
#define NAME_DRIVEAEXVALUE
void put_SendUserIO(const UserIO &value)
void put_HandIO(int value)
std::string RosName() const
#define NAME_DRIVEEXSTRING
#define BCAP_ROBOT_HALT_ARGS
void put_SendFormat(int format)
HRESULT CreatePoseData(const PoseData &pose, VARIANT &vnt)
std::vector< double > m_joint
int get_Timestamp() const
void Callback_DriveValue(const std::string &name, const DriveValueGoalConstPtr &goal)