static msg_t can_rx(void *p) {
struct can_instance *cip = p;
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
#if !SPC5_CAN_USE_FILTERS
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#else
while (canReceive(cip->canp, rxFlag,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#endif
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
static THD_FUNCTION(cancom_thread, arg) {
(void)arg;
chRegSetThreadName("CAN");
event_listener_t el;
CANRxFrame rxmsg;
uint8_t buffer[9];
chEvtRegister(&CANDx.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
continue;
}
while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
buffer[0] = rxmsg.SID;
for (int i = 0;i < rxmsg.DLC;i++) {
buffer[i + 1] = rxmsg.data8[i];
}
packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static void canRead(void) {
scheduleMsg(&logger, "waiting for CAN");
canReceive(&EFI_CAN_DEVICE, CAN_ANY_MAILBOX, &rxBuffer, TIME_INFINITE);
canReadCounter++;
printPacket(&rxBuffer);
obdOnCanPacketRx(&rxBuffer);
}
void QNetworkAccessDebugPipeBackend::socketReadyRead()
{
if (bareProtocol) {
qint64 bytesToRead = socket.bytesAvailable();
if (bytesToRead) {
QByteArray buffer;
buffer.resize(bytesToRead);
qint64 bytesRead = socket.read(buffer.data(), bytesToRead);
if (bytesRead < bytesToRead)
buffer.truncate(bytesRead);
writeDownstreamData(buffer);
readyReadEmitted = true;
}
return;
}
while (canReceive() &&
(socket.state() == QAbstractSocket::UnconnectedState || nextDownstreamBlockSize())) {
DataPacket packet;
if (receive(packet)) {
if (!packet.headers.isEmpty()) {
QList<QPair<QByteArray, QByteArray> >::ConstIterator
it = packet.headers.constBegin(),
end = packet.headers.constEnd();
for ( ; it != end; ++it)
setRawHeader(it->first, it->second);
metaDataChanged();
}
if (!packet.data.isEmpty()) {
writeDownstreamData(packet.data);
readyReadEmitted = true;
}
if (packet.headers.isEmpty() && packet.data.isEmpty()) {
// it's an eof
socket.close();
readyReadEmitted = true;
}
} else {
// got an error
QString msg = QObject::tr("Read error reading from %1: %2")
.arg(url().toString(), socket.errorString());
error(QNetworkReply::ProtocolFailure, msg);
finished();
return;
}
}
}
void can_irq_handler(void)
/******************************************************************************
CAN Interrupt
******************************************************************************/
{
volatile unsigned int status;
static Message m = Message_Initializer; // contain a CAN message
status = AT91F_CAN_GetStatus(AT91C_BASE_CAN) & AT91F_CAN_GetInterruptMaskStatus(AT91C_BASE_CAN);
if(status & RX_INT_MSK)
{ // Rx Interrupt
if (canReceive(&m)) // a message received
canDispatch(&ObjDict_Data, &m); // process it
}
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palTogglePad(GPIOE, GPIOE_LED3_RED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
bool QNetworkAccessDebugPipeBackend::receive(DataPacket &packet)
{
if (!canReceive())
return false;
// canReceive() does the setting up for us
Q_ASSERT(socket.bytesAvailable() >= incomingPacketSize);
QByteArray incomingPacket = socket.read(incomingPacketSize);
QDataStream stream(&incomingPacket, QIODevice::ReadOnly);
stream.setVersion(QDataStream::Qt_4_4);
stream >> packet.headers >> packet.data;
// reset for next packet:
incomingPacketSize = 0;
socket.setReadBufferSize(ReadBufferSize);
return true;
}
static msg_t can_rx (void * p) {
(void)p;
CANRxFrame rxmsg;
CANTxFrame txmsg;
txmsg.IDE = CAN_IDE_EXT;
txmsg.RTR = CAN_RTR_DATA;
txmsg.DLC = 8;
(void)p;
chRegSetThreadName("receiver");
while(!chThdShouldTerminate()) {
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, 100) == RDY_OK) {
canToBridge(&rxmsg);
if (!enableBootMaster)
continue;
/* Process message, with manual filtering */
if (rxmsg.IDE && (rxmsg.EID & 0x1FFFFF80) == 0x1F123480) {
if (rxmsg.DLC == 8) {
int i = 0;
while (i < nextNode)
if (memcmp(nodeMap[i], rxmsg.data8, 8) == 0)
break;
else
++i;
if (i >= nextNode)
memcpy(nodeMap[nextNode++], rxmsg.data8, 8);
chprintf(chp1, "\r\nCAN ann %08x: %08x %08x -> %d\r\n",
rxmsg.EID, rxmsg.data32[0], rxmsg.data32[1], i);
txmsg.EID = 0x1F123400 + i;
memcpy(txmsg.data8, rxmsg.data8, 8);
canTransmit(&CAND1, 1, &txmsg, 100);
echoToBridge(&txmsg);
} else if (rxmsg.DLC == 2) {
uint8_t dest = rxmsg.data8[0], page = rxmsg.data8[1];
chprintf(chp1, "CAN boot %08x: %02x #%d ", rxmsg.EID, dest, page);
if (sendFile(dest, rxmsg.EID & 0x7F, page))
chprintf(chp1, " SENT");
chprintf(chp1, "\r\n");
}
}
}
}
return 0;
}
static msg_t can_rx(void *p) {
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(IOPORT3, GPIOC_LED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
int main(void)
{
sys_init(); // Initialize system
canInit(CAN_BAUDRATE); // Initialize the CANopen bus
initTimer(); // Start timer for the CANopen stack
nodeID = read_bcd(); // Read node ID first
setNodeId (&ObjDict_Data, nodeID);
setState(&ObjDict_Data, Initialisation); // Init the state
for(;;) // forever loop
{
if (sys_timer) // Cycle timer, invoke action on every time slice
{
reset_sys_timer(); // Reset timer
digital_input[0] = get_inputs();
digital_input_handler(&ObjDict_Data, digital_input, sizeof(digital_input));
digital_output_handler(&ObjDict_Data, digital_output, sizeof(digital_output));
set_outputs(digital_output[0]);
// Check if CAN address has been changed
if(!( nodeID == read_bcd()))
{
nodeID = read_bcd(); // Save the new CAN adress
setState(&ObjDict_Data, Stopped); // Stop the node, to change the node ID
setNodeId(&ObjDict_Data, nodeID); // Now the CAN adress is changed
setState(&ObjDict_Data, Pre_operational); // Set to Pre_operational, master must boot it again
}
}
// a message was received pass it to the CANstack
if (canReceive(&m)) // a message reveived
canDispatch(&ObjDict_Data, &m); // process it
else
{
// Enter sleep mode
#ifdef WD_SLEEP // Watchdog and Sleep
wdt_reset();
sleep_enable();
sleep_cpu();
#endif // Watchdog and Sleep
}
}
}
static THD_FUNCTION(can_rx, p) {
struct can_instance *cip = p;
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palToggleLine(cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
uint8_t can_process()
{
uint8_t res = 0;
if(can_state.init_return!=RET_OK){
return 0;
}
while(canReceive(&rec_m)){
/***********This code will start any board which sends out a
pre-operational CANOpen code*/
// if(rec_m.cob_id&0x0F){
// if(rec_m.data[0] == 0x7F){
// can_enable_slave_heartbeat((UNS16)rec_m.cob_id&0x0FF,100);
// can_start_node(rec_m.cob_id&0x0FF);
// }
// }
canDispatch(&Sensor_Board_Data, &rec_m); //send packet to CanFestival
res = 1;
}
switch(getState(&Sensor_Board_Data)){
case Initialisation:
LED_2 = 0;
LED_3 = 0;
break;
case Pre_operational:
LED_2 = 1;
LED_3 = 0;
break;
case Operational:
LED_2 = 0;
LED_3 = 1;
break;
default:
LED_2 = 1;
LED_3 = 1;
break;
};
return res;
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
oca_led_toggle(oca_led_act);
CanMessage UsbMessage = CanMessage_init_default;
UsbMessage.DLC = rxmsg.DLC;
UsbMessage.RTR = rxmsg.DLC;
UsbMessage.IDE = rxmsg.IDE;
UsbMessage.ID = rxmsg.EID;
UsbMessage.Data1 = rxmsg.data32[0];
UsbMessage.Data1 = rxmsg.data32[1];
uint8_t buffer[30];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
pb_encode(&stream, CanMessage_fields, &UsbMessage);
/*if(serusbcfg.usbp->state == USB_ACTIVE)
{
if(SDU1.state == SDU_READY)
chnWrite(&SDU1, buffer, stream.bytes_written);
}*/
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
int Network_Connection::Recv(void* buf, int size) {
if (socketHandle >= 0) {
if (!canReceive()) return 0;
#ifdef WIN32
int bytes = recv(socketHandle, (char*)buf, size, 0);
#else
int bytes = recv(socketHandle, buf, size, MSG_NOSIGNAL | MSG_DONTWAIT);
#endif
if (bytes <= 0) { //FIXME: proper error handling
#ifdef WIN32
logWritem("network: socket read error %d, dropping (socket: %d)", GetLastError(), socketHandle);
#else
logWritem("network: socket read error %d, dropping (socket: %d)", errno, socketHandle);
#endif
bytes = 0;
Close();
}
Network.numBytesReceived += bytes;
return bytes;
} else return -1;
}
void Network_Connection::Update() {
if (listenConnection) {
struct sockaddr_in fromAddress;
memset(&fromAddress, 0, sizeof(fromAddress));
#ifdef _WIN32
int fromLen = sizeof(fromAddress);
#else
socklen_t fromLen = sizeof(fromAddress);
#endif
if (canReceive()) {
int clientHandle = accept(socketHandle, (struct sockaddr*)&fromAddress, &fromLen);
if (clientHandle < 0) return;
Network_Connection* clientConnection = Network.NewConnection();
if (!clientConnection) return;
clientConnection->Open(0,0); //Initialize resources
clientConnection->peerAddress = fromAddress;
clientConnection->socketHandle = clientHandle;
#ifdef WIN32
logWritem("network: incoming connection %.8X:%d (socket: %d, cid %d)",
fromAddress.sin_addr.S_un.S_addr, htons(fromAddress.sin_port), clientHandle, Network.Connections.Count - 1);
#else
logWritem("network: incoming connection %.8X:%d (socket: %d, cid %d)",
fromAddress.sin_addr.s_addr, htons(fromAddress.sin_port), clientHandle, Network.Connections.Count - 1);
#endif
}
} else {
LockID lockID = Thread.EnterLock(MUTEX_NETWORK_RECVBUF);
//A slightly hacky message receiver
//msgSize is set to how big message currently should be
//addSize is set after first byte was fetched, and says how much extra data needs to be fetched
// before msgSize will be reset from 1 (default) to actual message size (excluding additional data)
int readSize = 3;
while (canReceive() || (recvMessage.rawSize >= readSize)) {
if (recvMessage.rawSize > 0) { //Message already carries message ID
unsigned short msgSize = *(unsigned short*)((char*)recvMessage.rawBuffer+0);
unsigned char msgID = *((char*)recvMessage.rawBuffer+2);
if (msgSize > Network.maxMessageSize) msgSize = Network.maxMessageSize;
readSize = msgSize;
if (readSize == 0) return; //FIXME
if (recvMessage.rawSize >= readSize) {
if ((msgID < NETMESSAGE_LAST) && (Network.networkCallback[msgID])) {
Network.numPacketsReceived++;
recvMessage.rawPos = 3; //Skip message ID and size
Network.networkCallback[msgID](this,&recvMessage);
recvMessage.rawSize = 0;
readSize = 3; //get ready to recv more messages
} else { //Unhandled message, ignore
recvMessage.rawSize = 0;
}
}
}
//read more bytes to the message
if (recvMessage.rawSize < readSize) {
recvMessage.rawSize += Recv((recvMessage.rawBuffer+recvMessage.rawSize),readSize-recvMessage.rawSize);
}
}
Thread.LeaveLock(lockID);
}
}
CCM_FUNC static THD_FUNCTION(ThreadCAN, arg)
{
event_listener_t el;
CANTxFrame txmsg;
CANRxFrame rxmsg;
uint16_t i;
bool pidserved;
(void)arg;
chRegSetThreadName("CAN Bus");
while(CAND1.state != CAN_READY) chThdSleepMilliseconds(10);
while(SDU1.state != SDU_READY) chThdSleepMilliseconds(10);
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(TRUE) {
// Are we using coms for sensor data? If not just sleep.
if (settings.sensorsInput != SENSORS_INPUT_COM && (settings.functions & FUNC_OBD_SERVER) == 0) {
chThdSleepMilliseconds(100);
continue;
}
//checkCanFilters(&CAND1, &cancfg);
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
continue;
}
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
if (dbg_can) {
chprintf(DBG_STREAM, "->[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), rxmsg.SID, rxmsg.RTR);
for(i = 0; i < rxmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", rxmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
if (settings.functions & FUNC_OBD_SERVER) {
pidserved = serveCanOBDPidRequest(&CAND1, &txmsg, &rxmsg);
if (dbg_can && pidserved) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < txmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
else if (dbg_can) {
chprintf(DBG_STREAM, "!![CANBUS][%08x][ignored sender %03x]\r\n", chSysGetRealtimeCounterX(), rxmsg.SID);
}
}
if ((settings.functions & FUNC_OBD_SERVER) == 0) {
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN) {
readCanOBDPidResponse(&rxmsg);
}
else if (settings.sensorsInput == SENSORS_INPUT_YAMAHA_CAN) {
readCanYamahaPid(&rxmsg);
}
}
}
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN
&& (settings.functions & FUNC_OBD_SERVER) == 0) {
// Request OBD PIDs
sendCanOBDFrames(&CAND1, &txmsg);
if (dbg_can) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < 8; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
chThdSleepMilliseconds(100); // ~10Hz
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return;
}
/*
* kern_return_t
* bootstrap_check_in(mach_port_t bootstrap_port,
* name_t service_name,
* mach_port_t *service_portp)
*
* Returns receive rights to service_port of service named by service_name.
*
* Errors: Returns appropriate kernel errors on rpc failure.
* Returns BOOTSTRAP_UNKNOWN_SERVICE, if service does not exist.
* Returns BOOTSTRAP_SERVICE_NOT_DECLARED, if service not declared
* in /etc/bootstrap.conf.
* Returns BOOTSTRAP_SERVICE_ACTIVE, if service has already been
* registered or checked-in.
*/
kern_return_t
x_bootstrap_check_in(
mach_port_t bootstrap_port,
name_t service_name,
mach_port_t *service_portp)
{
kern_return_t result;
mach_port_t previous;
service_t *servicep;
server_t *serverp;
bootstrap_info_t *bootstrap;
serverp = lookup_server_by_port(bootstrap_port);
bootstrap = lookup_bootstrap_by_port(bootstrap_port);
debug("Service checkin attempt for service %s bootstrap %x",
service_name, bootstrap_port);
servicep = lookup_service_by_name(bootstrap, service_name);
if (servicep == NULL || servicep->port == MACH_PORT_NULL) {
debug("bootstrap_check_in service %s unknown%s", service_name,
forward_ok ? " forwarding" : "");
return forward_ok ?
bootstrap_check_in(
inherited_bootstrap_port,
service_name,
service_portp) :
BOOTSTRAP_UNKNOWN_SERVICE;
}
if (servicep->server != NULL && servicep->server != serverp) {
debug("bootstrap_check_in service %s not privileged",
service_name);
return BOOTSTRAP_NOT_PRIVILEGED;
}
if (!canReceive(servicep->port)) {
ASSERT(servicep->isActive);
debug("bootstrap_check_in service %s already active",
service_name);
return BOOTSTRAP_SERVICE_ACTIVE;
}
debug("Checkin service %s for bootstrap %x", service_name,
bootstrap->bootstrap_port);
ASSERT(servicep->isActive == FALSE);
servicep->isActive = TRUE;
if (servicep->server != NULL_SERVER) {
/* registered server - service needs backup */
serverp->activity++;
serverp->active_services++;
result = mach_port_request_notification(
mach_task_self(),
servicep->port,
MACH_NOTIFY_PORT_DESTROYED,
0,
backup_port,
MACH_MSG_TYPE_MAKE_SEND_ONCE,
&previous);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_request_notification");
} else {
/* one time use/created service */
servicep->servicetype = REGISTERED;
result = mach_port_request_notification(
mach_task_self(),
servicep->port,
MACH_NOTIFY_DEAD_NAME,
0,
notify_port,
MACH_MSG_TYPE_MAKE_SEND_ONCE,
&previous);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_request_notification");
else if (previous != MACH_PORT_NULL) {
debug("deallocating old notification port (%x) for checked in service %x",
previous, servicep->port);
result = mach_port_deallocate(
mach_task_self(),
previous);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_deallocate");
}
}
info("Check-in service %x in bootstrap %x: %s",
servicep->port, servicep->bootstrap->bootstrap_port, servicep->name);
*service_portp = servicep->port;
return BOOTSTRAP_SUCCESS;
}
int HandymanTeleopKey::run(int argc, char **argv)
{
char c;
/////////////////////////////////////////////
// get the console in raw mode
int kfd = 0;
struct termios cooked;
struct termios raw;
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
/////////////////////////////////////////////
showHelp();
ros::init(argc, argv, "handyman_teleop_key");
ros::NodeHandle node_handle;
// Override the default ros sigint handler.
// This must be set after the first NodeHandle is created.
signal(SIGINT, rosSigintHandler);
ros::Rate loop_rate(40);
std::string sub_msg_to_robot_topic_name;
std::string pub_msg_to_moderator_topic_name;
std::string sub_joint_state_topic_name;
std::string pub_base_twist_topic_name;
std::string pub_arm_trajectory_topic_name;
std::string pub_gripper_trajectory_topic_name;
node_handle.param<std::string>("sub_msg_to_robot_topic_name", sub_msg_to_robot_topic_name, "/handyman/message/to_robot");
node_handle.param<std::string>("pub_msg_to_moderator_topic_name", pub_msg_to_moderator_topic_name, "/handyman/message/to_moderator");
node_handle.param<std::string>("sub_joint_state_topic_name", sub_joint_state_topic_name, "/hsrb/joint_states");
node_handle.param<std::string>("pub_base_twist_topic_name", pub_base_twist_topic_name, "/hsrb/opt_command_velocity");
node_handle.param<std::string>("pub_arm_trajectory_topic_name", pub_arm_trajectory_topic_name, "/hsrb/arm_trajectory_controller/command");
node_handle.param<std::string>("pub_gripper_trajectory_topic_name", pub_gripper_trajectory_topic_name, "/hsrb/gripper_trajectory_controller/command");
ros::Subscriber sub_msg = node_handle.subscribe<handyman::HandymanMsg>(sub_msg_to_robot_topic_name, 100, &HandymanTeleopKey::messageCallback, this);
ros::Publisher pub_msg = node_handle.advertise<handyman::HandymanMsg>(pub_msg_to_moderator_topic_name, 10);
ros::Subscriber sub_joint_state = node_handle.subscribe<sensor_msgs::JointState>(sub_joint_state_topic_name, 10, &HandymanTeleopKey::jointStateCallback, this);
ros::Publisher pub_base_twist = node_handle.advertise<geometry_msgs::Twist> (pub_base_twist_topic_name, 10);
ros::Publisher pub_arm_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_arm_trajectory_topic_name, 10);
ros::Publisher pub_gripper_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_gripper_trajectory_topic_name, 10);
const float linear_coef = 0.2f;
const float linear_oblique_coef = 0.141f;
const float angular_coef = 0.5f;
float move_speed = 1.0f;
bool is_hand_open = false;
std::string arm_lift_joint_name = "arm_lift_joint";
std::string arm_flex_joint_name = "arm_flex_joint";
std::string wrist_flex_joint_name = "wrist_flex_joint";
while (ros::ok())
{
if(canReceive(kfd))
{
// get the next event from the keyboard
if(read(kfd, &c, 1) < 0)
{
perror("read():");
exit(EXIT_FAILURE);
}
switch(c)
{
case KEYCODE_0:
{
sendMessage(pub_msg, MSG_I_AM_READY);
break;
}
case KEYCODE_1:
{
sendMessage(pub_msg, MSG_ROOM_REACHED);
break;
}
case KEYCODE_2:
{
sendMessage(pub_msg, MSG_OBJECT_GRASPED);
break;
}
case KEYCODE_3:
{
sendMessage(pub_msg, MSG_TASK_FINISHED);
break;
}
case KEYCODE_6:
{
sendMessage(pub_msg, MSG_DOES_NOT_EXIST);
break;
}
case KEYCODE_9:
{
sendMessage(pub_msg, MSG_GIVE_UP);
break;
}
case KEYCODE_UP:
{
ROS_DEBUG("Go Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_DOWN:
{
ROS_DEBUG("Go Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_RIGHT:
{
ROS_DEBUG("Go Right");
moveBase(pub_base_twist, 0.0, 0.0, -angular_coef*move_speed);
break;
}
case KEYCODE_LEFT:
{
ROS_DEBUG("Go Left");
moveBase(pub_base_twist, 0.0, 0.0, +angular_coef*move_speed);
break;
}
case KEYCODE_S:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_U:
{
ROS_DEBUG("Move Left Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_I:
{
ROS_DEBUG("Move Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_O:
{
ROS_DEBUG("Move Right Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_J:
{
ROS_DEBUG("Move Left");
moveBase(pub_base_twist, 0.0, +linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_K:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_L:
{
ROS_DEBUG("Move Right");
moveBase(pub_base_twist, 0.0, -linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_M:
{
ROS_DEBUG("Move Left Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_COMMA:
{
ROS_DEBUG("Move Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_PERIOD:
{
ROS_DEBUG("Move Right Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_Q:
{
ROS_DEBUG("Move Speed Up");
move_speed *= 2;
if(move_speed > 2 ){ move_speed=2; }
break;
}
case KEYCODE_Z:
{
ROS_DEBUG("Move Speed Down");
move_speed /= 2;
if(move_speed < 0.125){ move_speed=0.125; }
break;
}
case KEYCODE_Y:
{
ROS_DEBUG("Move Arm Up");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.69, std::max<int>((int)(std::abs(0.69 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_H:
{
ROS_DEBUG("Stop Arm");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 2.0*arm_lift_joint_pos1_-arm_lift_joint_pos2_, 0.5);
break;
}
case KEYCODE_N:
{
ROS_DEBUG("Move Arm Down");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.0, std::max<int>((int)(std::abs(0.0 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_A:
{
ROS_DEBUG("Rotate Arm - Vertical");
moveArm(pub_arm_trajectory, arm_flex_joint_name, 0.0, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -1.57, 1);
break;
}
case KEYCODE_B:
{
ROS_DEBUG("Rotate Arm - Upward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -0.785, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -0.785, 1);
break;
}
case KEYCODE_C:
{
ROS_DEBUG("Rotate Arm - Horizontal");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -1.57, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.0, 1);
break;
}
case KEYCODE_D:
{
ROS_DEBUG("Rotate Arm - Downward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -2.2, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.35, 1);
break;
}
case KEYCODE_G:
{
moveHand(pub_gripper_trajectory, is_hand_open);
is_hand_open = !is_hand_open;
break;
}
}
}
ros::spinOnce();
loop_rate.sleep();
}
/////////////////////////////////////////////
// cooked mode
tcsetattr(kfd, TCSANOW, &cooked);
/////////////////////////////////////////////
return EXIT_SUCCESS;
}
/*
* kern_return_t
* bootstrap_register(mach_port_t bootstrap_port,
* name_t service_name,
* mach_port_t service_port)
*
* Registers send rights for the port service_port for the service named by
* service_name. Registering a declared service or registering a service for
* which bootstrap has receive rights via a port backup notification is
* allowed.
* The previous service port will be deallocated. Restarting services wishing
* to resume service for previous clients must first attempt to checkin to the
* service.
*
* Errors: Returns appropriate kernel errors on rpc failure.
* Returns BOOTSTRAP_NOT_PRIVILEGED, if request directed to
* unprivileged bootstrap port.
* Returns BOOTSTRAP_SERVICE_ACTIVE, if service has already been
* register or checked-in.
*/
kern_return_t
x_bootstrap_register(
mach_port_t bootstrap_port,
name_t service_name,
mach_port_t service_port)
{
kern_return_t result;
service_t *servicep;
server_t *serverp;
bootstrap_info_t *bootstrap;
mach_port_t old_port;
debug("Register attempt for service %s port %x",
service_name, service_port);
/*
* Validate the bootstrap.
*/
bootstrap = lookup_bootstrap_by_port(bootstrap_port);
if (!bootstrap || !active_bootstrap(bootstrap))
return BOOTSTRAP_NOT_PRIVILEGED;
/*
* If this bootstrap port is for a server, or it's an unprivileged
* bootstrap can't register the port.
*/
serverp = lookup_server_by_port(bootstrap_port);
servicep = lookup_service_by_name(bootstrap, service_name);
if (servicep && servicep->server && servicep->server != serverp)
return BOOTSTRAP_NOT_PRIVILEGED;
if (servicep == NULL || servicep->bootstrap != bootstrap) {
servicep = new_service(bootstrap,
service_name,
service_port,
ACTIVE,
REGISTERED,
NULL_SERVER);
debug("Registered new service %s", service_name);
} else {
if (servicep->isActive) {
debug("Register: service %s already active, port %x",
servicep->name, servicep->port);
ASSERT(!canReceive(servicep->port));
return BOOTSTRAP_SERVICE_ACTIVE;
}
old_port = servicep->port;
if (servicep->servicetype == DECLARED) {
servicep->servicetype = REGISTERED;
if (servicep->server) {
ASSERT(servicep->server == serverp);
ASSERT(active_server(serverp));
servicep->server = NULL_SERVER;
serverp->activity++;
}
result = mach_port_mod_refs(
mach_task_self(),
old_port,
MACH_PORT_RIGHT_RECEIVE,
-1);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_mod_refs");
}
result = mach_port_deallocate(
mach_task_self(),
old_port);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_mod_refs");
servicep->port = service_port;
servicep->isActive = TRUE;
debug("Re-registered inactive service %x bootstrap %x: %s",
servicep->port, servicep->bootstrap->bootstrap_port, service_name);
}
/* detect the new service port going dead */
result = mach_port_request_notification(
mach_task_self(),
service_port,
MACH_NOTIFY_DEAD_NAME,
0,
notify_port,
MACH_MSG_TYPE_MAKE_SEND_ONCE,
&old_port);
if (result != KERN_SUCCESS) {
debug("Can't request notification on service %x bootstrap %x: %s",
service_port, servicep->bootstrap->bootstrap_port, "must be dead");
delete_service(servicep);
return BOOTSTRAP_SUCCESS;
} else if (old_port != MACH_PORT_NULL) {
debug("deallocating old notification port (%x) for service %x",
old_port, service_port);
result = mach_port_deallocate(
mach_task_self(),
old_port);
if (result != KERN_SUCCESS)
kern_fatal(result, "mach_port_deallocate");
}
info("Registered service %x bootstrap %x: %s",
servicep->port, servicep->bootstrap->bootstrap_port, servicep->name);
return BOOTSTRAP_SUCCESS;
}
