Skip to content
Permalink
Browse files

key message

  • Loading branch information...
vickailiu committed Sep 8, 2018
1 parent 9397f47 commit 8d7810574594900a9622a9e9a2d83a72bdae7673
Showing with 109 additions and 8 deletions.
  1. +15 −0 include/ur_modern_driver/robot_state.h
  2. +93 −8 src/robot_state.cpp
  3. +1 −0 src/ur_ros_wrapper.cpp
@@ -109,6 +109,17 @@ struct version_message {
char build_date[25];
};

struct key_message {
uint64_t timestamp;
int8_t source;
int8_t robot_message_type;
int8_t robot_message_code;
int8_t robot_message_argument;
unsigned char title_size;
char message_title[64];
char text_message[64];
};

struct masterboard_data {
int digitalInputBits;
int digitalOutputBits;
@@ -150,6 +161,7 @@ struct robot_mode_data {

class RobotState {
private:
key_message key_msg_;
version_message version_msg_;
masterboard_data mb_data_;
robot_mode_data robot_mode_;
@@ -166,6 +178,7 @@ class RobotState {
RobotState(std::condition_variable& msg_cond);
~RobotState();
double getVersion();
key_message getKeyMessage();
double getTime();
std::vector<double> getQTarget();
int getDigitalInputBits();
@@ -210,6 +223,8 @@ class RobotState {
void unpackRobotMessage(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
uint32_t len);
void unpackRobotMessageKey(uint8_t * buf, unsigned int offset, unsigned int message_offset,
uint32_t len);
void unpackRobotState(uint8_t * buf, unsigned int offset, uint32_t len);
void unpackRobotStateMasterboard(uint8_t * buf, unsigned int offset);
void unpackRobotMode(uint8_t * buf, unsigned int offset);
@@ -64,24 +64,35 @@ void RobotState::unpack(uint8_t* buf, unsigned int buf_length) {

void RobotState::unpackRobotMessage(uint8_t * buf, unsigned int offset,
uint32_t len) {
offset += 5;
unsigned int message_offset = 5;
uint64_t timestamp;
int8_t source, robot_message_type;
memcpy(&timestamp, &buf[offset], sizeof(timestamp));
offset += sizeof(timestamp);
memcpy(&source, &buf[offset], sizeof(source));
offset += sizeof(source);
memcpy(&robot_message_type, &buf[offset], sizeof(robot_message_type));
offset += sizeof(robot_message_type);
memcpy(&timestamp, &buf[offset+message_offset], sizeof(timestamp));
message_offset += sizeof(timestamp);
memcpy(&source, &buf[offset+message_offset], sizeof(source));
message_offset += sizeof(source);
memcpy(&robot_message_type, &buf[offset+message_offset], sizeof(robot_message_type));
message_offset += sizeof(robot_message_type);

// printf("%lu: received message with type %d and length %lu\n", timestamp, robot_message_type, len);

switch (robot_message_type) {
case robotMessageType::ROBOT_MESSAGE_VERSION:
val_lock_.lock();
version_msg_.timestamp = timestamp;
version_msg_.source = source;
version_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageVersion(buf, offset, len);
RobotState::unpackRobotMessageVersion(buf, offset+message_offset, len);
val_lock_.unlock();
break;
case robotMessageType::ROBOT_MESSAGE_KEY:
val_lock_.lock();
key_msg_.timestamp = timestamp;
key_msg_.source = source;
key_msg_.robot_message_type = robot_message_type;
RobotState::unpackRobotMessageKey(buf, offset, message_offset, len);
val_lock_.unlock();
break;
default:
break;
}
@@ -146,6 +157,69 @@ void RobotState::unpackRobotMessageVersion(uint8_t * buf, unsigned int offset,
}
}

void RobotState::unpackRobotMessageKey(uint8_t * buf, unsigned int offset, unsigned int message_offset,
uint32_t len) {
// memcpy(&key_msg_.robot_message_code, &buf[offset+message_offset], sizeof(key_msg_.robot_message_code));
// key_msg_.robot_message_code = ntohl(key_msg_.robot_message_code);
// message_offset += sizeof(key_msg_.robot_message_code);

// memcpy(&key_msg_.robot_message_argument, &buf[offset+message_offset], sizeof(key_msg_.robot_message_argument));
// key_msg_.robot_message_argument = ntohl(key_msg_.robot_message_argument);
// message_offset += sizeof(key_msg_.robot_message_argument);

// memcpy(&key_msg_.title_size, &buf[offset+message_offset], sizeof(key_msg_.title_size));
// message_offset += sizeof(key_msg_.title_size);
// int title_size = key_msg_.title_size;

// memcpy(&key_msg_.message_title, &buf[offset+message_offset], sizeof(char) * title_size);
// message_offset += title_size;
// key_msg_.message_title[key_msg_.title_size] = '\0';
// memcpy(&key_msg_.text_message, &buf[offset+message_offset], len - message_offset);
// key_msg_.text_message[len - message_offset] = '\0';
// printf("%d, %d, %d, %s, %s\n",
// key_msg_.robot_message_code, key_msg_.robot_message_argument,
// title_size, key_msg_.message_title,
// key_msg_.text_message);


int robot_message_code, robot_message_argument;

memcpy(&robot_message_code, &buf[offset+message_offset], sizeof(robot_message_code));
robot_message_code = ntohl(robot_message_code);
message_offset += sizeof(robot_message_code);
key_msg_.robot_message_code = robot_message_code;

memcpy(&robot_message_argument, &buf[offset+message_offset], sizeof(robot_message_argument));
robot_message_code = ntohl(robot_message_argument);
message_offset += sizeof(robot_message_argument);
key_msg_.robot_message_argument = robot_message_argument;

unsigned char title_size;
memcpy(&title_size, &buf[offset+message_offset], sizeof(title_size));
message_offset += sizeof(title_size);
key_msg_.title_size = title_size;
int size = title_size;

//printf("%d, %d, %d, %d\n",key_msg_.robot_message_code, key_msg_.robot_message_argument,key_msg_.title_size, size);

char message_title[64];
memcpy(&message_title, &buf[offset+message_offset], sizeof(char) * size);
message_offset += size;
message_title[size] = '\0';
//printf("%s\n", message_title);
strcpy(key_msg_.message_title,message_title);
//printf("%s\n",key_msg_.message_title);


char text_message[64];
memcpy(&text_message, &buf[offset+message_offset], len - message_offset);
text_message[254] = '\0';
//printf("%s\n", text_message);
strcpy(key_msg_.text_message,text_message);
//printf("%s\n",key_msg_.text_message);

}

void RobotState::unpackRobotMode(uint8_t * buf, unsigned int offset) {
memcpy(&robot_mode_.timestamp, &buf[offset], sizeof(robot_mode_.timestamp));
offset += sizeof(robot_mode_.timestamp);
@@ -322,6 +396,17 @@ double RobotState::getVersion() {

}

key_message RobotState::getKeyMessage(){
key_message new_key_msg;
val_lock_.lock();
strcpy(new_key_msg.message_title,key_msg_.message_title);
strcpy(new_key_msg.text_message,key_msg_.text_message);
memset(key_msg_.message_title,'\0',64);
memset(key_msg_.text_message,'\0',64);
val_lock_.unlock();
return new_key_msg;
}

void RobotState::finishedReading() {
new_data_available_ = false;
}
@@ -68,6 +68,7 @@ class RosWrapper {
control_msgs::FollowJointTrajectoryResult result_;
ros::Subscriber speed_sub_;
ros::Subscriber urscript_sub_;
ros::Subscriber key_sub;
ros::ServiceServer io_srv_;
ros::ServiceServer payload_srv_;
std::thread* rt_publish_thread_;

0 comments on commit 8d78105

Please sign in to comment.
You can’t perform that action at this time.