int main(int argc, char ** argv)
{
struct sigaction sa;
bzero(&sa, sizeof(sa));
sa.sa_handler = handle;
if (0 != sigaction(SIGINT, &sa, 0)) {
err(EXIT_FAILURE, "sigaction");
}
// Before we attempt to read any tasks and skills from the YAML
// file, we need to inform the static type registry about custom
// additions such as the HelloGoodbyeSkill.
Factory::addSkillType<uta_opspace::RigidTf>("uta_opspace::RigidTf");
Factory::addSkillType<uta_opspace::SurfaceMotion>("uta_opspace::SurfaceMotion");
Factory::addSkillType<uta_opspace::SurfaceOriMotion>("uta_opspace::SurfaceOriMotion");
Factory::addSkillType<uta_opspace::AttachSurface>("uta_opspace::AttachSurface");
Factory::addSkillType<uta_opspace::KnownAttachSurface>("uta_opspace::KnownAttachSurface");
Factory::addSkillType<uta_opspace::JointMultiPos>("uta_opspace::JointMultiPos");
ros::init(argc, argv, "wbc_fri_servo", ros::init_options::NoSigintHandler);
parse_options(argc, argv);
ros::NodeHandle node("~");
jspace::Vector command(Vector::Zero(7));
pos_prev = Vector::Zero(7);
//Need to setup the transform here
//Should be changed to a parameter in TAO
jspace::Matrix R(Matrix::Identity(3,3));
R(0,0) = 0.9221;
R(0,1) = -0.0576;
R(0,2) = -0.3827;
R(1,0) = 0.3791;
R(1,1) = -0.0643;
R(1,2) = 0.9231;
R(2,0) = -0.0778;
R(2,1) = -0.9963;
R(2,2) = -0.0374;
jspace::Matrix d(Matrix::Zero(3,1));
/*d(0,0) = -1.0877;
d(1,0) = 2.8646;
d(2,0) = 0.0528;*/
d(0,0) = -1.0877;
d(1,0) = 3.0146;
d(2,0) = 0.0428;
controller.reset(new ControllerNG("wbc_fri::servo"));
param_cbs.reset(new ParamCallbacks());
Servo servo;
if (verbose) {
warnx("initializing param callbacks");
}
registry.reset(factory->createRegistry());
registry->add(controller);
param_cbs->init(node, registry, 1, 100);
//Initial message
ros::Publisher torque_pub = node.advertise<JointEfforts>("/JointEffortCommand", 1000);
ros::Publisher jointimp_pub = node.advertise<FriJointImpedance>("/FriJointImpedance", 1000);
ros::Subscriber state_sub = node.subscribe("/JointState", 1000, stateCallback);
ros::Subscriber mass_sub = node.subscribe("/MassMatrix", 1000, massCallback);
ros::Subscriber fristate_sub = node.subscribe("/FriJointState", 1000, friCallback);
JointEfforts torque_msg;
FriJointImpedance jointimp_msg;
for (size_t ii(0); ii<7; ++ii) {
torque_msg.efforts.push_back(0.0);
jointimp_msg.stiffness[ii] = 0.0;
jointimp_msg.damping[ii] = 0.0;
}
torque_pub.publish(torque_msg);
jointimp_pub.publish(jointimp_msg);
//UDP camera- with option of no camera
Position3d rawCamData[56];
jspace::Matrix camData;
//EDIT ME to actual camera address
char local_cam_port [10];
sprintf(local_cam_port, "%d", CLIENT_PORT+2);
char cam_port [10];
sprintf(cam_port, "%d", CS8C_PORT+2);
UdpOSI cameraUDP("50000", "192.168.1.39", cam_port, 0);
int bytes =0;
if (cam) {
bytes = cameraUDP.recvPacket((char*)rawCamData, sizeof(rawCamData));
if (bytes<1) {
fprintf(stderr, "Error receiving data from camera");
servo.cleanup();
return 0;
}
camData = Matrix::Zero(bytes/sizeof(Position3d),3);
for (size_t ii=0; ii < bytes/sizeof(Position3d); ++ii){
camData(ii,0) = rawCamData[ii].x*1e-3;
camData(ii,1) = rawCamData[ii].y*1e-3;
camData(ii,2) = rawCamData[ii].z*1e-3;
}
Matrix temp(R * camData.transpose() + d*Matrix::Ones(1,bytes/sizeof(Position3d)));
state.camData_ = temp.transpose();
cout <<"Recieved initial camera data\n";
}
/*
//Visualization
visinfo info;
UdpOSI visUDP( "57860", const_cast<char*>(CS8C_IPADDR), "50000", 0);*/
int status = servo.init(state);
if (0!= status) {
fprintf(stderr, "init callback returned %d\n", status);
servo.cleanup();
return 0;
}
ros::Time dbg_t0(ros::Time::now());
ros::Time dump_t0(ros::Time::now());
ros::Duration dbg_dt(0.1);
ros::Duration dump_dt(0.05);
//ros::Rate loop_rate(10);
ros::Time begin(ros::Time::now());
while (ros::ok()) {
//UDP camera in and out
if (cam) {
bytes = cameraUDP.recvPacket((char*)rawCamData, sizeof(rawCamData));
if (bytes <1) {
fprintf(stderr,"Error: no camera data recieved");
servo.cleanup();
return 0;
}
for (size_t ii=0; ii < bytes/sizeof(Position3d); ++ii){
camData(ii,0) = rawCamData[ii].x*1e-3;
camData(ii,1) = rawCamData[ii].y*1e-3;
camData(ii,2) = rawCamData[ii].z*1e-3;
}
Matrix temp(R * camData.transpose() + d*Matrix::Ones(1,bytes/sizeof(Position3d)));
state.camData_ = temp.transpose();
}
/*
//Vis out
for (size_t ii(0); ii<6 ; ++ii) {
info.q[ii] = state.position_[ii];
}
Skill::task_table_t const * tasks(servo.skill->getTaskTable());
info.R[0] = 0.1;
info.T[0] = 0.4;//No longer automatic-> should not be manual
if (cam) {
for (size_t ii(0); ii < state.camData_.rows(); ++ii) {
for (size_t jj(0); jj < state.camData_.cols(); ++jj) {
info.sp[ii][jj] = state.camData_(ii,jj);
}
}
}
info.eedes[0] = 0.0;
info.eedes[1] = 0.0;
info.eedes[2] = 0.0;
info.cp[0] = 0.0;
info.cp[1] = 0.0;
info.cp[2] = 0.0;
bytes = visUDP.sendPacket((char*)&info, sizeof(info));
*/
status = servo.update(state, command);
if (0 != status) {
fprintf(stderr, "update callback returned %d\n", status);
servo.cleanup();
return 0;
}
for (size_t ii(0); ii<7; ++ii) {
if (isnan(command[ii])) {
torque_msg.efforts[ii] = 0;
}
else {
torque_msg.efforts[ii] = command[ii];
}
}
torque_pub.publish(torque_msg);
jointimp_pub.publish(jointimp_msg);
ros::Time t1(ros::Time::now());
if (verbose) {
if (t1 - dbg_t0 > dbg_dt) {
dbg_t0 = t1;
servo.skill->dbg(cout, "\n\n**************************************************", "");
controller->dbg(cout, "--------------------------------------------------", "");
cout << "--------------------------------------------------\n";
jspace::pretty_print(model->getState().position_, cout, "jpos", " ");
jspace::pretty_print(controller->getCommand(), cout, "gamma", " ");
jspace::pretty_print(model->getState().camData_, cout, "cam", " ");
}
}
if (t1 - dump_t0 > dump_dt) {
dump_t0 = t1;
controller->qhlog(*servo.skill, ros::Duration(t1-begin).toSec()*1000);
}
ros::spinOnce();
//loop_rate.sleep();
}
warnx("shutting down");
ros::shutdown();
servo.cleanup();
//cameraUDP.~UdpOSI();
}
