int main(int argc, char** argv){
ros::init(argc, argv, "coax_webots_tf_broadcaster");
if (argc < 2) {
printf("Usage: %s <host>\n",argv[0]);
return -1;
}
SBChannel pose_channel;
sbChannelCreateSocketUDP(&pose_channel,argv[1],5125);
sbChannelOpen(&pose_channel);
sbChannelSend(&pose_channel,(const unsigned char*)argv[0],strlen(argv[0]));
printf("Opened pose channel on UDP ports 5125\n");
ros::NodeHandle node;
tf::TransformBroadcaster br;
while (ros::ok()) {
SBSerialisedMessage reply;
SB6DofPoseMessage pose;
ros::spinOnce();
if (sbChannelWaitData(&pose_channel, 100)) {
continue;
}
// printf("\n1");
if (sbWaitRawMessage(&pose_channel,-1,&reply,100)) {
continue;
}
// printf("2");
if ((reply.msgid & SB_MSGID_MASK) != SB_MSGID_6DOF_POSE) {
continue;
}
// printf("3");
if (sb6DofPoseDecode(&reply,&pose)) {
continue;
}
//printf("[%ld %ld %ld] [%d %d %d]\n",
// pose.x,pose.y,pose.z,pose.roll,pose.pitch,pose.yaw);
// printf("4");
// fflush(stdout);
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin( tf::Vector3(pose.x/1000., pose.y/1000., pose.z/1000.) );
q.setRPY(pose.roll*M_PI/0x8000,pose.pitch*M_PI/0x8000,pose.yaw*M_PI/0x8000);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "coax"));
}
return 0;
};
// Open a serial communication channel to the helicopter.
// Under linux, device may be (for instance) "/dev/tty0" or "/dev/rfcomm0" for
// bluetooth, and baudrate is a define symbol as defined in bit/termios.h (e.g.
// B115200).
// TODO: implement and test under windows/osx
int sbOpenBluetoothCommunication(struct SBControlContext *control,
const char *port, unsigned int speed)
{
int res;
memset(control, 0, sizeof(struct SBControlContext));
res = sbChannelCreateBTWin(&control->channel,port,speed);
if (res) {
return res;
}
res = sbChannelOpen(&control->channel);
// by default, request acknowledgement
control->ackMode = SB_MSGID_REQACK;
return res;
}
// Open a network communication channel to the helicopter.
// This may be used in simulation or possibly to communicate via an on-board
// network-enabled processor.
int sbOpenWinsockCommunication(struct SBControlContext *control,
const char *hostname, unsigned int port)
{
int res;
memset(control, 0, sizeof(struct SBControlContext));
res = sbChannelCreateWinsockUDP(&control->channel,hostname,port);
if (res) {
return res;
}
res = sbChannelOpen(&control->channel);
// by default, request acknowledgement
control->ackMode = SB_MSGID_REQACK;
return res;
}
// Open a serial communication channel to the helicopter.
// Under linux, device may be (for instance) "/dev/tty0" or "/dev/rfcomm0" for
// bluetooth, and baudrate is a define symbol as defined in bit/termios.h (e.g.
// B115200).
// TODO: implement and test under windows/osx
int sbOpenSerialCommunication(struct SBControlContext *control,
const char *device, unsigned int baudrate, int rtscts)
{
int res;
bzero(control, sizeof(struct SBControlContext));
res = sbChannelCreateSerial(&control->channel,device,baudrate,rtscts);
if (res) {
return res;
}
res = sbChannelOpen(&control->channel);
// by default, request acknowledgement
control->ackMode = SB_MSGID_REQACK;
return res;
}