Client class for PG70 serial control. More...
#include <pg70_rs232_control.h>
Public Member Functions | |
bool | acknowledgeErrorCallback (schunk_pg70::acknowledge_error::Request &req, schunk_pg70::acknowledge_error::Response &res) |
AcknowledgeError service callback. | |
bool | getErrorCallback (schunk_pg70::get_error::Request &req, schunk_pg70::get_error::Response &res) |
GetError service callback. | |
bool | getPositionCallback (schunk_pg70::get_position::Request &req, schunk_pg70::get_position::Response &res) |
GetPosition service callback. | |
PG70_serial (ros::NodeHandle *nh) | |
Construct a client for PG70 serial control. | |
bool | referenceCallback (schunk_pg70::reference::Request &req, schunk_pg70::reference::Response &res) |
Reference service callback. | |
bool | setPositionCallback (schunk_pg70::set_position::Request &req, schunk_pg70::set_position::Response &res) |
SetPosition service callback. | |
bool | stopCallback (schunk_pg70::stop::Request &req, schunk_pg70::stop::Response &res) |
Stop service callback. | |
void | timerCallback (const ros::TimerEvent &event) |
Timer callback to read serial input buffer periodically. | |
~PG70_serial () | |
Public Attributes | |
ros::Publisher | joint_pub |
Gripper joint state publisher. | |
Static Public Attributes | |
static const float | TF_UPDATE_PERIOD = 0.1 |
TF update period in seconds. | |
Private Member Functions | |
void | acknowledgeError (serial::Serial *port) |
Send CMD_ACK(0x8b) command to the gripper. | |
uint16_t | CRC16 (uint16_t crc, uint16_t data) |
Function to determine checksum. | |
void | float_to_IEEE_754 (float position, unsigned int *output_array) |
Conversion from float to 4 bytes. | |
uint8_t | getError (serial::Serial *port) |
Read actual error by GET STATE(0x95) command. | |
void | getPeriodicPositionUpdate (serial::Serial *port, float update_frequency) |
Set periodic position reading by GET_STATE(0x95) command. | |
float | getPosition (serial::Serial *port) |
Read actual position by GET_STATE(0x95) command. | |
float | IEEE_754_to_float (uint8_t *raw) |
Conversion from 4 bytes to float. | |
void | reference (serial::Serial *port) |
Send CMD REFERENCE(0x92) command to the gripper. | |
void | setPosition (serial::Serial *port, int goal_position, int velocity, int acceleration) |
Send MOV_POS(0x80) command to the gripper. | |
void | stop (serial::Serial *port) |
Send CMD_STOP(0x91) to stop moving gripper. | |
Private Attributes | |
float | act_position_ |
int | baudrate_ |
serial::Serial * | com_port_ |
int | gripper_id_ |
uint8_t | pg70_error_ |
sensor_msgs::JointState | pg70_joint_state_ |
std::string | port_name_ |
Static Private Attributes | |
static const double | INPUT_BUFFER_SIZE = 64 |
static const double | MAX_GRIPPER_ACC_LIMIT = 320 |
static const double | MAX_GRIPPER_POS_LIMIT = 69 |
static const double | MAX_GRIPPER_VEL_LIMIT = 83 |
static const double | MIN_GRIPPER_ACC_LIMIT = 0 |
static const double | MIN_GRIPPER_POS_LIMIT = 0 |
static const double | MIN_GRIPPER_VEL_LIMIT = 0 |
static const int | URDF_SCALE_FACTOR = 2000 |
static const double | WAIT_FOR_RESPONSE_INTERVAL = 0.5 |
Client class for PG70 serial control.
Definition at line 54 of file pg70_rs232_control.h.
Construct a client for PG70 serial control.
Definition at line 41 of file pg70_rs232_control_lib.cpp.
Definition at line 70 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::acknowledgeError | ( | serial::Serial * | port | ) | [private] |
Send CMD_ACK(0x8b) command to the gripper.
Definition at line 189 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::acknowledgeErrorCallback | ( | schunk_pg70::acknowledge_error::Request & | req, |
schunk_pg70::acknowledge_error::Response & | res | ||
) |
AcknowledgeError service callback.
Definition at line 435 of file pg70_rs232_control_lib.cpp.
uint16_t schunk_pg70::PG70_serial::CRC16 | ( | uint16_t | crc, |
uint16_t | data | ||
) | [private] |
Function to determine checksum.
Definition at line 533 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::float_to_IEEE_754 | ( | float | position, |
unsigned int * | output_array | ||
) | [private] |
Conversion from float to 4 bytes.
Definition at line 524 of file pg70_rs232_control_lib.cpp.
uint8_t schunk_pg70::PG70_serial::getError | ( | serial::Serial * | port | ) | [private] |
Read actual error by GET STATE(0x95) command.
Definition at line 100 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::getErrorCallback | ( | schunk_pg70::get_error::Request & | req, |
schunk_pg70::get_error::Response & | res | ||
) |
GetError service callback.
Definition at line 427 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::getPeriodicPositionUpdate | ( | serial::Serial * | port, |
float | update_frequency | ||
) | [private] |
Set periodic position reading by GET_STATE(0x95) command.
Definition at line 316 of file pg70_rs232_control_lib.cpp.
float schunk_pg70::PG70_serial::getPosition | ( | serial::Serial * | port | ) | [private] |
Read actual position by GET_STATE(0x95) command.
Definition at line 260 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::getPositionCallback | ( | schunk_pg70::get_position::Request & | req, |
schunk_pg70::get_position::Response & | res | ||
) |
GetPosition service callback.
Definition at line 419 of file pg70_rs232_control_lib.cpp.
float schunk_pg70::PG70_serial::IEEE_754_to_float | ( | uint8_t * | raw | ) | [private] |
Conversion from 4 bytes to float.
Definition at line 507 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::reference | ( | serial::Serial * | port | ) | [private] |
Send CMD REFERENCE(0x92) command to the gripper.
Definition at line 77 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::referenceCallback | ( | schunk_pg70::reference::Request & | req, |
schunk_pg70::reference::Response & | res | ||
) |
Reference service callback.
Definition at line 375 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::setPosition | ( | serial::Serial * | port, |
int | goal_position, | ||
int | velocity, | ||
int | acceleration | ||
) | [private] |
Send MOV_POS(0x80) command to the gripper.
Definition at line 212 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::setPositionCallback | ( | schunk_pg70::set_position::Request & | req, |
schunk_pg70::set_position::Response & | res | ||
) |
SetPosition service callback.
Definition at line 383 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::stop | ( | serial::Serial * | port | ) | [private] |
Send CMD_STOP(0x91) to stop moving gripper.
Definition at line 348 of file pg70_rs232_control_lib.cpp.
bool schunk_pg70::PG70_serial::stopCallback | ( | schunk_pg70::stop::Request & | req, |
schunk_pg70::stop::Response & | res | ||
) |
Stop service callback.
Definition at line 448 of file pg70_rs232_control_lib.cpp.
void schunk_pg70::PG70_serial::timerCallback | ( | const ros::TimerEvent & | event | ) |
Timer callback to read serial input buffer periodically.
Definition at line 458 of file pg70_rs232_control_lib.cpp.
float schunk_pg70::PG70_serial::act_position_ [private] |
Definition at line 134 of file pg70_rs232_control.h.
int schunk_pg70::PG70_serial::baudrate_ [private] |
Definition at line 131 of file pg70_rs232_control.h.
Definition at line 139 of file pg70_rs232_control.h.
int schunk_pg70::PG70_serial::gripper_id_ [private] |
Definition at line 129 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::INPUT_BUFFER_SIZE = 64 [static, private] |
Definition at line 149 of file pg70_rs232_control.h.
Gripper joint state publisher.
Definition at line 91 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MAX_GRIPPER_ACC_LIMIT = 320 [static, private] |
Definition at line 147 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MAX_GRIPPER_POS_LIMIT = 69 [static, private] |
Definition at line 143 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MAX_GRIPPER_VEL_LIMIT = 83 [static, private] |
Definition at line 145 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MIN_GRIPPER_ACC_LIMIT = 0 [static, private] |
Definition at line 146 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MIN_GRIPPER_POS_LIMIT = 0 [static, private] |
Definition at line 142 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::MIN_GRIPPER_VEL_LIMIT = 0 [static, private] |
Definition at line 144 of file pg70_rs232_control.h.
uint8_t schunk_pg70::PG70_serial::pg70_error_ [private] |
Definition at line 135 of file pg70_rs232_control.h.
sensor_msgs::JointState schunk_pg70::PG70_serial::pg70_joint_state_ [private] |
Definition at line 136 of file pg70_rs232_control.h.
std::string schunk_pg70::PG70_serial::port_name_ [private] |
Definition at line 130 of file pg70_rs232_control.h.
const float schunk_pg70::PG70_serial::TF_UPDATE_PERIOD = 0.1 [static] |
TF update period in seconds.
Definition at line 94 of file pg70_rs232_control.h.
const int schunk_pg70::PG70_serial::URDF_SCALE_FACTOR = 2000 [static, private] |
Definition at line 150 of file pg70_rs232_control.h.
const double schunk_pg70::PG70_serial::WAIT_FOR_RESPONSE_INTERVAL = 0.5 [static, private] |
Definition at line 148 of file pg70_rs232_control.h.