Commit eb51cfdd authored by Mathias Haage's avatar Mathias Haage

Initial check-in of egm communication code

parent 517af69a
all: egm_client_mockup
egm.pb.cc egm.pb.h : egm.proto
protoc --cpp_out=. egm.proto
egm_client_mockup : egm.pb.cc egm.pb.h egm_client_mockup.cpp
gcc -O3 -std=c++11 egm_client_mockup.cpp egm.pb.cc -o egm_client_mockup -lboost_system -lboost_thread -lboost_chrono -lprotobuf -lstdc++ -lm -lpthread
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
// Definition of ABB sensor interface V1.0
//
// messages of type EgmRobot are sent out from the robot controller
// messages of type EgmSensor are sent to the robot controller
//
package abb.egm;
message EgmHeader
{
optional uint32 seqno = 1; // sequence number (to be able to find lost messages)
optional uint32 tm = 2; // time stamp in milliseconds
enum MessageType {
MSGTYPE_UNDEFINED = 0;
MSGTYPE_COMMAND = 1; // for future use
MSGTYPE_DATA = 2; // sent by robot controller
MSGTYPE_CORRECTION = 3; // sent by sensor
}
optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED];
}
message EgmCartesian // Cartesian position interpreted relative to the sensor frame defined by EGMActPose
{
required double x = 1;
required double y = 2;
required double z = 3;
}
message EgmQuaternion // Quaternion orientation interpreted relative to the sensor frame defined by EGMActPose
{
required double u0 = 1;
required double u1 = 2;
required double u2 = 3;
required double u3 = 4;
}
message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) interpreted relative to the sensor frame defined by EGMActPose
{
optional EgmCartesian pos = 1;
optional EgmQuaternion orient = 2;
}
message EgmCartesianSpeed {
repeated double value = 1;
}
message EgmJoints // Array of 6 joint values for TCP robot
{
repeated double joints = 1;
}
message EgmExternalJoints // Array of 6 joint values for additional axis
{
repeated double joints = 1;
}
message EgmPlanned // Planned position for robot (joints or cartesian) and additional axis (array of 6 values)
{
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
}
message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values)
{
optional EgmJoints joints = 1;
optional EgmCartesianSpeed cartesians = 2;
optional EgmJoints externalJoints = 3;
}
message EgmFeedBack // Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values)
{
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
}
message EgmMotorState // Motor state
{
enum MotorStateType {
MOTORS_UNDEFINED = 0;
MOTORS_ON = 1;
MOTORS_OFF = 2;
}
required MotorStateType state = 1;
}
message EgmMCIState // EGM state
{
enum MCIStateType {
MCI_UNDEFINED = 0;
MCI_ERROR = 1;
MCI_STOPPED = 2;
MCI_RUNNING = 3;
}
required MCIStateType state = 1 [default = MCI_UNDEFINED];
}
message EgmRapidCtrlExecState // RAPID execution state
{
enum RapidCtrlExecStateType {
RAPID_UNDEFINED = 0;
RAPID_STOPPED = 1;
RAPID_RUNNING = 2;
};
required RapidCtrlExecStateType state = 1 [default = RAPID_UNDEFINED];
}
message EgmTestSignals // Test signals
{
repeated double signals = 1;
}
// Robot controller outbound message
message EgmRobot
{
optional EgmHeader header = 1;
optional EgmFeedBack feedBack = 2;
optional EgmPlanned planned = 3;
optional EgmMotorState motorState = 4;
optional EgmMCIState mciState = 5;
optional bool mciConvergenceMet = 6;
optional EgmTestSignals testSignals = 7;
optional EgmRapidCtrlExecState rapidExecState = 8;
}
// Robot controller inbound message, sent from sensor
message EgmSensor
{
optional EgmHeader header = 1;
optional EgmPlanned planned = 2;
optional EgmSpeedRef speedRef = 3;
}
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <boost/asio.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/interprocess/sync/interprocess_mutex.hpp>
#include "egm.pb.h" // Generated by Google protoc.exe
//#pragma comment(lib, "libprotobuf.lib") // Protobuf lib
using boost::asio::ip::udp;
int counter = 0;
void fillEgmRobot(abb::egm::EgmRobot &robot_message) {
// EgmHeader
robot_message.mutable_header()->set_seqno(++counter);
robot_message.mutable_header()->set_tm(counter);
robot_message.mutable_header()->set_mtype(abb::egm::EgmHeader_MessageType_MSGTYPE_DATA);
// EgmFeedBack
for (int i=0; i<6; ++i) {
robot_message.mutable_feedback()->mutable_joints()->add_joints(counter);
robot_message.mutable_feedback()->mutable_externaljoints()->add_joints(counter);
}
robot_message.mutable_feedback()->mutable_cartesian()->mutable_pos()->set_x(counter);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_pos()->set_y(counter);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_pos()->set_z(counter);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_orient()->set_u0(1);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_orient()->set_u1(0);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_orient()->set_u2(0);
robot_message.mutable_feedback()->mutable_cartesian()->mutable_orient()->set_u3(0);
// EgmPlanned
for (int i=0; i<6; ++i) {
robot_message.mutable_planned()->mutable_joints()->add_joints(counter);
robot_message.mutable_planned()->mutable_externaljoints()->add_joints(counter);
}
robot_message.mutable_planned()->mutable_cartesian()->mutable_pos()->set_x(counter);
robot_message.mutable_planned()->mutable_cartesian()->mutable_pos()->set_y(counter);
robot_message.mutable_planned()->mutable_cartesian()->mutable_pos()->set_z(counter);
robot_message.mutable_planned()->mutable_cartesian()->mutable_orient()->set_u0(1);
robot_message.mutable_planned()->mutable_cartesian()->mutable_orient()->set_u1(0);
robot_message.mutable_planned()->mutable_cartesian()->mutable_orient()->set_u2(0);
robot_message.mutable_planned()->mutable_cartesian()->mutable_orient()->set_u3(0);
// EgmMotorState
robot_message.mutable_motorstate()->set_state(abb::egm::EgmMotorState_MotorStateType_MOTORS_ON);
// EgmMCIState
robot_message.mutable_mcistate()->set_state(abb::egm::EgmMCIState_MCIStateType_MCI_RUNNING);
// mciConvergenceMet
robot_message.set_mciconvergencemet(true);
// EgmTestSignals
robot_message.mutable_testsignals()->add_signals((double)counter);
// EgmRapidCtrlExecState
robot_message.mutable_rapidexecstate()->set_state(abb::egm::EgmRapidCtrlExecState_RapidCtrlExecStateType_RAPID_RUNNING);
}
void printEgmRobot(abb::egm::EgmRobot &robot_message) {
std::cout << "EgmRobot ";
if (robot_message.has_feedback()) {
abb::egm::EgmFeedBack in_p = robot_message.feedback();
// Position references
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "p[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "pe[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
/*
// Speed references
if (robot_message.feedback().has_jointspeed()) {
abb::egm::EgmJointSpace in_p = robot_message.feedback().jointspeed();
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "s[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "se[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
}
// Torque references
if (robot_message.feedback().has_jointtorque()) {
abb::egm::EgmJointSpace in_p = robot_message.feedback().jointtorque();
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "t[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "te[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
*/
}
std::cout << std::endl;
}
void printEgmSensor(abb::egm::EgmSensor &sensor_message) {
std::cout << "EgmSensor ";
// Position references
if (sensor_message.has_planned()) {
abb::egm::EgmPlanned in_p = sensor_message.planned();
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "p[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "pe[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
}
// Speed references
if (sensor_message.has_speedref()) {
abb::egm::EgmSpeedRef in_p = sensor_message.speedref();
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "s[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "se[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
}
/*
// Torque references
if (sensor_message.has_torquefeedforward()) {
abb::egm::EgmJointSpace in_p = sensor_message.torquefeedforward();
if (in_p.has_joints()) {
for (size_t i = 0; i < (size_t)in_p.joints().joints_size(); ++i) {
std::cout << "t[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
if (in_p.has_externaljoints()) {
for (size_t i = 0; i < (size_t)in_p.externaljoints().joints_size(); ++i) {
std::cout << "te[" << i << "]=" << in_p.joints().joints(i) << ",";
}
}
}
*/
std::cout << std::endl;
}
int main()
{
static int const buffer_size = 1024;
std::array<char, buffer_size> receive_buffer;
std::string send_buffer;
abb::egm::EgmRobot robot_message_left;
abb::egm::EgmSensor sensor_message_left;
//abb::egm::EgmRobot robot_message_right;
//abb::egm::EgmSensor sensor_message_right;
std::cerr << "Starting EGMRI client mockup" << std::endl;
boost::asio::io_service io_service;
udp::resolver resolver(io_service);
udp::resolver::query query_left(udp::v4(), "localhost", "6511");
udp::endpoint receiver_endpoint_left = *resolver.resolve(query_left);
//udp::resolver::query query_right(udp::v4(), "localhost", "6512");
//udp::endpoint receiver_endpoint_right = *resolver.resolve(query_right);
udp::endpoint sender_endpoint;
udp::socket socket_left(io_service);
//udp::socket socket_right(io_service);
socket_left.open(udp::v4());
//socket_right.open(udp::v4());
while (true) {
//std::cout << "Hello" << std::endl;
std::cout << "ROBOT" << std::endl;
robot_message_left.Clear();
fillEgmRobot(robot_message_left);
printEgmRobot(robot_message_left);
robot_message_left.SerializeToString(&send_buffer);
socket_left.send_to(boost::asio::buffer(send_buffer), receiver_endpoint_left);
//size_t len = socket_left.receive_from(boost::asio::buffer(receive_buffer), sender_endpoint);
//std::string message_left(receive_buffer.begin(), receive_buffer.end());
//sensor_message_left.Clear();
//sensor_message_left.ParseFromArray(message_left.c_str(), len);
//printEgmSensor(sensor_message_left);
//std::cout << "RIGHT" << std::endl;
//robot_message_right.Clear();
//fillEgmRobot(robot_message_right);
//printEgmRobot(robot_message_right);
//robot_message_right.SerializeToString(&send_buffer);
//socket_right.send_to(boost::asio::buffer(send_buffer), receiver_endpoint_right);
//len = socket_right.receive_from(boost::asio::buffer(receive_buffer), sender_endpoint);
//std::string message_right(receive_buffer.begin(), receive_buffer.end());
//sensor_message_right.Clear();
//sensor_message_right.ParseFromArray(message_right.c_str(), len);
//printEgmSensor(sensor_message_right);
}
return 0;
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment