31 static unsigned int kResponseOffset = 0x580;
32 static unsigned int kRequestOffset = 0x600;
60 if(
driver_->getState().isReady())
63 serial_request.
id =
can_id_ + kRequestOffset;
64 serial_request.
dlc = (
unsigned char)8;
68 serial_request.
data.fill((
unsigned char)0);
70 serial_request.
data[0] = 0x2F;
71 serial_request.
data[1] = 0x00;
72 serial_request.
data[2] = 0x22;
74 serial_request.
data[3] = 0x01;
75 serial_request.
data[4] = 0x00;
76 serial_request.
data[4] |= (output.rACT & 0x1) << 0;
77 serial_request.
data[4] |= (output.rMOD & 0x3) << 1;
78 serial_request.
data[4] |= (output.rGTO & 0x1) << 3;
79 serial_request.
data[4] |= (output.rATR & 0x1) << 4;
86 serial_request.
data[3] = 0x02;
87 serial_request.
data[4] = 0x00;
88 serial_request.
data[4] |= (output.rICF & 0x1) << 2;
89 serial_request.
data[4] |= (output.rICS & 0x1) << 3;
96 unsigned char subindex = 0x04;
98 serial_request.
data[3] = subindex++;
99 serial_request.
data[4] = 0x00;
100 serial_request.
data[4] = output.rPRA;
107 serial_request.
data[3] = subindex++;
108 serial_request.
data[4] = 0x00;
109 serial_request.
data[4] = output.rSPA;
116 serial_request.
data[3] = subindex++;
117 serial_request.
data[4] = 0x00;
118 serial_request.
data[4] = output.rFRA;
125 serial_request.
data[3] = subindex++;
126 serial_request.
data[4] = 0x00;
127 serial_request.
data[4] = output.rPRB;
134 serial_request.
data[3] = subindex++;
135 serial_request.
data[4] = 0x00;
136 serial_request.
data[4] = output.rSPB;
143 serial_request.
data[3] = subindex++;
144 serial_request.
data[4] = 0x00;
145 serial_request.
data[4] = output.rFRB;
152 serial_request.
data[3] = subindex++;
153 serial_request.
data[4] = 0x00;
154 serial_request.
data[4] = output.rPRC;
161 serial_request.
data[3] = subindex++;
162 serial_request.
data[4] = 0x00;
163 serial_request.
data[4] = output.rSPC;
170 serial_request.
data[3] = subindex++;
171 serial_request.
data[4] = 0x00;
172 serial_request.
data[4] = output.rFRC;
179 serial_request.
data[3] = subindex++;
180 serial_request.
data[4] = 0x00;
181 serial_request.
data[4] = output.rPRS;
188 serial_request.
data[3] = subindex++;
189 serial_request.
data[4] = 0x00;
190 serial_request.
data[4] = output.rSPS;
197 serial_request.
data[3] = subindex++;
198 serial_request.
data[4] = 0x00;
199 serial_request.
data[4] = output.rFRS;
213 if(
driver_->getState().isReady())
216 serial_request.
id =
can_id_ + kRequestOffset;
217 serial_request.
dlc =
static_cast<unsigned char>(8);
219 serial_request.
is_rtr = 0;
221 serial_request.
data.fill(static_cast<unsigned char>(0));
223 serial_request.
data[0] = 0x4F;
224 serial_request.
data[1] = 0x00;
225 serial_request.
data[2] = 0x20;
227 for(
unsigned int i = 1; i < 16; ++i)
229 serial_request.
data[3] = i;
230 if(!
read_mutex.timed_lock(boost::posix_time::time_duration(0,0,1,0)))
232 std::stringstream err;
233 err<<
"unable to read response on index "<<i;
235 throw std::runtime_error(err.str().c_str());
240 if(!
read_mutex.timed_lock(boost::posix_time::time_duration(0,0,1,0)))
242 std::stringstream err;
243 err<<
"unable to read response on last index";
245 throw std::runtime_error(err.str().c_str());
269 throw std::runtime_error(err);
280 std::stringstream err;
281 err <<
"Invalid frame from SocketCAN: id " <<std::hex << f.
id <<
", length "<<(int) f.
dlc <<
", is_extended: "<<f.
is_extended<<
", is error: "<<f.
is_error<<
", is_rtr: "<<f.
is_rtr;
282 throw std::runtime_error(err.str().c_str());
286 throw std::runtime_error(
"Received frame is error");
288 if ((
int)frame.
dlc != 8)
290 throw std::runtime_error(
"Message length must be 8");
297 else if (f.
data[0]==0x4f && f.
data[1]==0x00 && f.
data[2] == 0x20)
299 switch((
int)f.
data[3])
363 iFrame.
dlc = (
unsigned char)2;
364 iFrame.
data[0] = 0x01;
371 input_.gACT = ((f >> 0) & 0x1);
372 input_.gMOD = ((f >> 1) & 0x3);
373 input_.gGTO = ((f >> 3) & 0x1);
374 input_.gIMC = ((f >> 4) & 0x3);
375 input_.gSTA = ((f >> 6) & 0x3);
380 input_.gDTA = ((f >> 0) & 0x3);
381 input_.gDTB = ((f >> 2) & 0x3);
382 input_.gDTC = ((f >> 4) & 0x3);
383 input_.gDTS = ((f >> 6) & 0x3);
388 input_.gFLT = ((f >> 0) & 0xF);
void decodeFingerBCurrent(const u_int8_t &f)
void decodeFingerCPosCmd(const u_int8_t &f)
void decodeObjectStatus(const u_int8_t &f)
virtual ~Robotiq3FGripperCanClient()
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput GripperOutput
can::Frame::Header resp_header_
void decodeFingerSCurrent(const u_int8_t &f)
void decodeFingerACurrent(const u_int8_t &f)
void decodeFingerBPos(const u_int8_t &f)
void decodeFingerAPosCmd(const u_int8_t &f)
std::map< unsigned char, unsigned char > prevCmd_
void decodeFingerAPos(const u_int8_t &f)
boost::array< value_type, 8 > data
void decodeFaultStatus(const u_int8_t &f)
void decodeFingerBPosCmd(const u_int8_t &f)
GripperOutput readOutputs() const
Reads set of output-register values from the gripper.
void stateCallback(const can::State &s)
void init(ros::NodeHandle nh)
can::StateInterface::StateListenerConstSharedPtr state_listener_
can::CommInterface::FrameListenerConstSharedPtr frame_listener_
void decodeFingerCCurrent(const u_int8_t &f)
void decodeGripperStatus(const u_int8_t &f)
#define ROS_INFO_STREAM(args)
Robotiq3FGripperCanClient(unsigned int can_id, boost::shared_ptr< can::DriverInterface > driver)
void decodeFingerSPosCmd(const u_int8_t &f)
boost::timed_mutex read_mutex
void decodeFingerSPos(const u_int8_t &f)
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput GripperInput
unsigned int internal_error
#define ROS_ERROR_STREAM(args)
void decodeFingerCPos(const u_int8_t &f)
void writeOutputs(const GripperOutput &output)
Write the given set of control flags to the memory of the gripper.
GripperInput readInputs() const
Reads set of input-register values from the gripper.
boost::shared_ptr< can::DriverInterface > driver_
boost::system::error_code error_code
void frameCallback(const can::Frame &f)