void update_mavlink(void){
mavlink_message_t msg;
mavlink_status_t status;
if(ser.available()){
uint16_t num_bytes = ser.available();
uint8_t buffer[num_bytes];
ser.read(buffer, num_bytes);
for(uint16_t i = 0; i < num_bytes; i++){
if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {
handleMessage(&msg, MAVLINK_COMM_0);
}
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "syncboard_node");
ros::NodeHandle nh;
Move mover;
// Advertise topics
ros::Publisher chatter_pub = nh.advertise<geometry_msgs::Twist>("/camera_stitch/cmd_vel", 1);
ros::Publisher timestamps_pub = nh.advertise<std_msgs::String>("syncbox_timestamps", 1);
//Synboard stuff
std::string port;
int baud;
nh.param<std::string>("port", port, "/dev/ttyUSB0");
nh.param("baud", baud, 9600);
syncboard.setTimeout(serial::Timeout::max(), 500, 0, 500, 0);
syncboard.setPort(port);
syncboard.setBaudrate(baud);
syncboard.open();
mover.handshake();
while(ros::ok()){
ros::spinOnce();
ros::Rate(10).sleep();
}
syncboard.write("s");
return 0;
}
bool checkConnection(){
if(!port.isOpen() && !port.getPort().empty()){
try{
port.open();
ROS_WARN_THROTTLE(5, "Serial port was closed, reopened.");
} catch (const serial::IOException& ex){
ROS_ERROR_THROTTLE(10, "Serial port is closed, reopening failed: %s", ex.what());
}
}
}
bool open() {
bool device_opened = false;
if (is_open()) {
if (debug)
std::cout << "serial already opened" << std::endl;
} else {
m_serial.setPort(m_device_port);
m_serial.setBaudrate(115200);
//serial::Timeout t(1000, 1000);
m_serial.setTimeout(serial::Timeout::simpleTimeout(2000));
m_serial.open();
// Open port
if (m_serial.isOpen()) {
m_serial.flush();
// Check if the device is well a Wattsup
if (identify()) {
m_serial.flush();
device_opened = true;
} else {
m_serial.close();
}
}
}
return device_opened;
}
bool Move::burst_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
static unsigned int count=0;
res.success = true;
res.message = "Started syncboard burst trigger"; //this trigger defaults to 5hz and is independent of robot position
geometry_msgs::Twist output;
output.linear.x = 0.2;
output.linear.y = 0.0;
output.linear.z = 0.0;
output.angular.x = 0.0;
output.angular.y = 0.0;
output.angular.z = 0.0;
movePub.publish(output);
syncboard.write("b");
usleep(1700000); //TUNE THIS PARAMETER
output.linear.x = 0.0;
output.linear.y = 0.0;
output.linear.z = 0.0;
output.angular.x = 0.0;
output.angular.y = 0.0;
output.angular.z = 0.0;
movePub.publish(output);
ROS_INFO("Finished the burst");
count++;
return true;
}
static void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
if (chan == MAVLINK_COMM_0)
{
ser.write(&ch, 1);
}
}
bool Move::handshake(){
int counter = 0;
while(ros::ok()){
syncboard.flush();
syncboard.write("v");
std::string result = syncboard.readline();
if (result.length() > 0){
ROS_INFO("Connected to syncboard.");
return true;
}
if(counter++ > 50){
ROS_WARN_ONCE("Connecting to syncboard is taking longer than expected.");
}
ros::Rate(10).sleep();
}
ROS_WARN("Syncboard handshake failed.");
return false;
}
void sendCommand(int robotId, int leftMotorSpeed, int rightMotorSpeed)
{
char cmd[4];
cmd[0] = robotId;
cmd[1] = 128 + leftMotorSpeed;
cmd[2] = 128 + rightMotorSpeed;
cmd[3] = '\n';
string cmdStr(cmd, cmd + 4);
my_serial.write(cmdStr);
}
// Opens and configures the com-port
void configure_com_port(void)
{
if (my_serial.isOpen())
{
cout << "Port is open " << endl;
}
else
{
cout << "Unable to open com-port " << endl;
}
}
// Get all bytes available
bool getBytes(char * buffer, int& buf_len) {
bool res = false;
//buf_len = 0;
try {
while (true) {
int res = 0;
size_t available = m_serial.available();
if (!available) {
break;
} else {
res = m_serial.read((uint8_t*)buffer + buf_len, available);
}
if (res <= 0) {
break;
}
buf_len += res;
m_last_read_time = Datapoint::get_current_time();
}
} catch (...) {
if (debug) {
std::cout << "exception thrown while wattsup reading" << std::endl;
}
close();
}
if (buf_len > 0) {
res = true;
}
return res;
}
bool write(Packet& p) {
if (!is_open()) {
close();
return false;
}
memset(p.m_buf, 0, sizeof(p.m_buf));
int n = sprintf(p.m_buf, "#%c,%c,%d", p.m_cmd, p.m_sub_cmd, p.m_count);
char* s = p.m_buf + n;
p.m_len = n;
for (int i = 0; i < p.m_count; i++) {
if ((p.m_len + strlen(p.m_fields[i]) + 4) >= sizeof(p.m_buf)) {
return false;
}
n = sprintf(s, ",%s", p.m_fields[i]);
s += n;
p.m_len += n;
}
p.m_buf[p.m_len++] = ';';
if (debug) {
cout << "[debug-write]: " << m_device_port << ": "
<< p.m_buf << endl;
}
try {
int pos = 0;
while (pos < p.m_len) {
int res = m_serial.write((const uint8_t *)p.m_buf + pos, p.m_len - pos);
if (res < 0) {
return false;
}
pos += res;
}
} catch (...) {
if (debug) {
std::cout << "exception thrown while wattsup writing" << std::endl;
}
close();
}
return true;
}
int main(int argc, char **argv){
ros::init(argc, argv, "nayan_ros");
ros::NodeHandle n;
imu_pub = n.advertise<nayan_msgs::nayan_imu>("nayan_ros/imu", 1000);
debug_var_pub = n.advertise<nayan_msgs::nayan_dbg>("nayan_ros/debug_var", 1000);
gps_pose_vel_pub = n.advertise<nayan_msgs::nayan_gps_pose_vel>("nayan_ros/gps_pose_vel", 1000);
attitude_pub = n.advertise<nayan_msgs::nayan_attitude>("nayan_ros/attitude", 1000);
rc_in_pub = n.advertise<nayan_msgs::nayan_rc_in>("nayan_ros/rc_in", 1000);
vision_est_sub = n.subscribe("nayan_ros/vision_estimate", 1000, update_vision_estimate);
pose_vel_setpt_sub = n.subscribe("nayan_ros/setpoint_pose_vel", 1000, update_setpoint_pose_vel);
param_srv = n.advertiseService("param_set", update_param);
ros::Duration(2).sleep();
if (ser.isOpen()){
ROS_INFO("Serial Port Opened!");
}else{
ROS_ERROR("Serial Port Cannot be Opened!");
}
ros::Rate loop_rate(100);
while(ros::ok()){
update_mavlink();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void close() {
if (m_closing) return;
m_closing = true;
if (is_open()) {
// Internal logging 600s
Packet p;
WattsupCommand::SetupInternalLogging600s(p);
write(p);
m_serial.flush();
}
m_serial.close();
m_buf_len = 0;
m_commands.clear();
m_closing = false;
//this->fileLog("Device closed ! [" + m_device_port + "]");
}
void receive(){
if(!checkConnection()){
return;
}
// reads:
// 3 bytes:
// 1 byte to compare with START_BYTE
// 1 bytes into _msg->len
// 1 byte into _msg->type
// len+1 bytes:
// len bytes into _msg->data
// 1 byte to compare with _msg->checksum
size_t amount;
while((amount=port.available()) > 0){
if(_pos > 0){
amount = min((unsigned int)amount, _msg->length + 3 - _pos);
port.read(&(_msg->data[_pos]), amount);
_pos += amount;
if(_pos == _msg->length + 3 && port.available()){
uint8_t checksum;
port.read(&checksum, 1);
_msg->calcChecksum();
if(checksum == _msg->checksum){
parse(_msg);
}else{
ROS_WARN_THROTTLE(1, "Serial: Checksum mismatch: %u != %u", checksum, _msg->checksum);
}
resetMessage();
}
}else if(amount >= 3){
uint8_t b;
port.read(&b, 1);
if(b != START_BYTE){
ROS_WARN_THROTTLE(1, "Serial: Expected start byte, got this instead: %u", b);
return;
}
uint8_t d[2];
port.read(d, 2);
uint16_t len = d[0];
if(len > 0){
_msg = new Message(len - 1, d[1]); // TODO catch exception?
_pos = 3;
}
}
}
}
// Sends the given channel values with it's checksum to
// the rc-controller
void send_to_controller( const QuadroController::channel_values::ConstPtr& msg)
{
send_data[0] = 0x00;
send_data[1] = 0x01;
send_data[2] = msg->channel_1;
send_data[3] = msg->channel_2;
send_data[4] = msg->channel_3;
send_data[5] = msg->channel_4;
send_data[6] = msg->channel_5;
send_data[7] = 0x00;
send_data[8] = 0x00;
send_data[9] = calculate_checksum(send_data,9);
// 0x55 is the start-byte to initiate the communication
// prozess
send_data[0] = 0x55;
my_serial.write(send_data,sizeof(send_data));
// Uncomment to see the checksum
//ROS_INFO("Checksum: [%d]", send_data[9]);
}
bool open(string portname, int baudrate){
port.setPort(portname);
port.setBaudrate(baudrate);
serial::Timeout t = serial::Timeout::simpleTimeout(1000);
port.setTimeout(t);
try{
port.open();
} catch (const serial::IOException& ex){
ROS_ERROR_THROTTLE(10, "IOException while attempting to open serial port '%s': %s", portname.c_str(), ex.what());
}
if(port.isOpen()){
port.setPort("kbot_bridge");
return true;
}
return false;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "doa_rssi_server");
ros::NodeHandle n2;
tf::TransformListener listener;
ros::Rate r(10);
ros::ServiceServer service = n2.advertiseService("get_doa_rssi", add);
ros::Publisher marker_pub_path = n2.advertise<visualization_msgs::Marker>("beacon_point", 10);
tf::StampedTransform transformMtoR;
tf::StampedTransform transform;
string dataString;
float doarssi[3];
bool doarssiready = false;
//ros::spin();//a blocking call duhh!
ROS_INFO("ready to give DOA and RSSI!!");
visualization_msgs::Marker markerDOA,markerIntersections,markerBeacon1,markerBeacon2,markerPath;
markerDOA.header.frame_id = "/map";
markerDOA.header.stamp = ros::Time::now();
markerDOA.ns = "markers";
markerDOA.action = visualization_msgs::Marker::ADD;
markerDOA.pose.orientation.w = 1.0;
markerDOA.id = 0;
markerDOA.type = visualization_msgs::Marker::POINTS;
//########POINTS markers use x and y scale for width/height respectively
markerDOA.scale.x = 1.2;
markerDOA.scale.y = 1.2;
markerDOA.scale.z = 0.5;
markerDOA.color.r = 1.0f;
markerDOA.color.b = 1.0f;
markerDOA.color.a = 1.0;
geometry_msgs::Point marker_point;
marker_point.x = 19.0;
marker_point.y = 19.0;
marker_point.z = 0.0;
markerDOA.points.push_back(marker_point);
marker_pub_path.publish(markerDOA);
while(n2.ok())
{
#ifndef SDR
try{
listener.lookupTransform("base_link", "beacon",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
phi = atan2f(transform.getOrigin().y(),transform.getOrigin().x());
rssi = 1/(transform.getOrigin().x()*transform.getOrigin().x()+transform.getOrigin().y()*transform.getOrigin().y());
#else
if(robotserial.available())
{
dataString = robotserial.readline(100,"\n");
cout << "odom received: "<< dataString << endl;
char * dataStringArray = new char [dataString.length()+1];
std::strcpy (dataStringArray, dataString.c_str());
//printf("%s\n",dataStringArray);
// dataStringArray now contains a c-string copy of str
char * token = std::strtok (dataStringArray,",");
char *unconverted;
int i = 0;
if(*token=='t')
{
while (token!=0)
{
//std::cout << token << "-" << atoi(token) <<'\n';
doarssi[i] = atof(token);
token = std::strtok(NULL,",");
i++;
}
doarssiready = true;
}
}
#endif
if(doarssiready==true)
{
phi = doarssi[1];
rssi = doarssi[2];
doarssiready = false;
}
ros::spinOnce();
marker_pub_path.publish(markerDOA);
}
return 0;
}
bool is_open() {
return m_serial.isOpen();
}
bool Move::trigger_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
{
syncboard.write("t");
return true;
}
void transmit(const uint8_t* d, size_t l){
if(!checkConnection()){
return;
}
port.write(d, l);
}
void RSDPA10::run()
{
// ----------- PA10 Robot Main State Machine -----------
plc_status = PLCserial.read(s+500);
if (plc_status == "t")
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SAFETY_SYSTEM_TRIPPED);
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SAFETY_SYSTEM_BLINK);
plc_status = "";
}
if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_WAIT)
{
set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_WAIT);
if(mes_state == FREE)
rw::common::Log::log().info() << "In MES State: " << "FREE" << std::endl;
else if (mes_state == OUT_OF_BRICKS)
rw::common::Log::log().info() << "In MES State: " << "OUT_OF_BRICKS" << std::endl;
}
else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_SORTBRICKS)
{
set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_SORTBRICKS);
rw::common::Log::log().info() << "In MES State: " << "SORTING" << std::endl;
mes_state = SORTING;
}
else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_LOADBRICKS)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_ORDER_COMPLETE);
set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_LOADBRICKS);
rw::common::Log::log().info() << "In MES State: " << "LOADING" << std::endl;
mes_state = LOADING;
}
else if(get_rsd_info(INFO_TYPE_MES_COMMAND) & MES_COMMAND_ABORT)
{
set_rsd_info(INFO_TYPE_MES_COMMAND, INFO_ACTION_CLEAR, MES_COMMAND_ABORT);
rw::common::Log::log().info() << "In MES State: " << "FREE" << std::endl;
mes_state = FREE;
}
if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_ORDER_COMPLETE)
{
rw::common::Log::log().info() << "In MES State: " << "ORDER_SORTED" << std::endl;
mes_state = ORDER_SORTED;
}
else if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_OUT_OF_BRICKS)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_OUT_OF_BRICKS);
rw::common::Log::log().info() << "In MES State: " << "OUT_OF_BRICKS" << std::endl;
mes_state = OUT_OF_BRICKS;
}
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_RESETJOINTS)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_RESETJOINTS);
linearPath.clear();
rw::common::Log::log().info() << "In State: " << "RESET" << std::endl;
mes_state = SORTING;
control_state = RESET;
}
switch(mes_state)
{
case INIT_ROS:
rw::common::Log::log().info() << "In MES State: " << "ROS_INIT" << std::endl;
_ROS->start();
mes_state = FREE;
break;
case FREE:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_LOADING);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ERROR);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_OUTOFBRICKS);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ORDERSORTED);
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_ORDER_COMPLETE);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_FREE);
break;
case SORTING:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_FREE);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_SORTING);
if(!orderQueue.empty() && !workingOnOrder)
{
currentOrder = orderQueue.front();
nrOfBricksInOrder = currentOrder.get_blueBricks() + currentOrder.get_redBricks() + currentOrder.get_yellowBricks();
workingOnOrder = true;
startBatchTime = ros::Time::now().toSec();
LINFO << "Working on Order: " << currentOrder.get_orderID();
}
switch (control_state)
{
case INIT:
{
if (linearPath.empty())
{
rw::common::Log::log().info() << "In State: " << "INIT" << std::endl;
Q gripperInitOpenQ(1, 0.020);
sendQtoGripper(gripperInitOpenQ);
//Start conveyor belt
PLCserial.write("f2");
// Issue move command for moving robot to ready position and switch state to MOVING_TO_READY_POS
Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6");
MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>();
Vector3D<> newPos(-0.60, 0.135, 0.007);
RPY<> newRot(0, 0, -Pi);
Transform3D<> newTarget = Transform3D<>(newPos, newRot);
objectMovFrame->setTransform(newTarget, _state);
Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state);
// Clear any previous planned path before generating a new
linearPath.clear();
moveToTarget(baseToObject, -0.15, false);
}
//if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) {
target_type = READY;
moveRobot();
last_state = INIT;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_READY_POS" << std::endl;
control_state = MOVING_TO_READY_POS;
//}
}
break;
case MOVING_TO_READY_POS:
// Is ready position is reached?
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_READY_REACHED) {
Q gripperInitOpenQ(1, 0.020);
sendQtoGripper(gripperInitOpenQ);
linearPath.clear();
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_READY_REACHED);
target_type = NONE;
last_state = MOVING_TO_READY_POS;
rw::common::Log::log().info() << "In State: " << "GET_TARGETS" << std::endl;
control_state = GET_TARGETS;
}
break;
case GET_TARGETS:
if(currentOrder.isOrderSorted()) {
rw::common::Log::log().info() << "Order is sorted!"<< std::endl;
PLCserial.write("f2");
conveyorStoppedTimeSaved = false;
conveyorHasStopped = false;
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING);
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_ORDER_COMPLETE);
if(!orderQueue.empty())
orderQueue.pop();
visionQueue.clear();
workingOnOrder = false;
LINFO << "Order " << currentOrder.get_orderID() <<" sorted successfully";
endBatchTime = ros::Time::now().toSec();
batchTime = endBatchTime - startBatchTime;
break;
}
else if(conveyorHasStopped && visionQueue.empty())
{
PLCserial.write("f2");
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING);
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_OUT_OF_BRICKS);
LINFO << "Out of bricks for Order " << currentOrder.get_orderID();
conveyorStoppedTimeSaved = false;
conveyorHasStopped = false;
}
if(!visionQueue.empty())
{
std::cout << "Queue size is : " << visionQueue.size() << std::endl;
nextBrick = visionQueue.front();
std::cout << "target is " << nextBrick.getBrickTransform() << std::endl;
std::cout << "timestamp: " << nextBrick.getTimestamp() << std::endl;
std::cout << "color: " << nextBrick.getColor() << std::endl;
std::cout << "speed: " << nextBrick.getSpeed() << std::endl;
std::cout << "Queue size after dequeue is : " << visionQueue.size() << std::endl;
last_state = GET_TARGETS;
rw::common::Log::log().info() << "In State: " << "WAITING_FOR_BRICK" << std::endl;
control_state = WAITING_FOR_BRICK;
}
break;
case MOVING_TO_BRICK_READY:
// If brick ready position target is reached?
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BRICK_READY_REACHED) {
linearPath.clear();
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BRICK_READY_REACHED);
double currentTime;
if(conveyorHasStopped)
{
currentTime = conveyorStoppedTime;
}
else
{
currentTime = ros::Time::now().toSec();
}
double timeDiff = currentTime - nextBrick.getTimestamp();
for(int i=0; i<conveyorToBrick.R().m().size1(); i++)
for(int j=0; j<conveyorToBrick.R().m().size2(); j++)
if(fabs(0-conveyorToBrick.R()(i,j)) < 0.00001)
conveyorToBrick.R()(i,j) = 0.0;
Rotation3D<> rotX14(1.0, 0.0, 0.0,
0.0, cos(14*Deg2Rad), -sin(14*Deg2Rad),
0.0, sin(14*Deg2Rad), cos(14*Deg2Rad));
Rotation3D<> rotXneg14(1.0, 0.0, 0.0,
0.0, cos(-14*Deg2Rad), -sin(-14*Deg2Rad),
0.0, sin(-14*Deg2Rad), cos(-14*Deg2Rad));
Vector3D<> brickPos = conveyorToBrick.P();
Rotation3D<> brickRot(conveyorToBrick.R());
/*if(brickPos(1) > 18.5)
brickRot = rotX14*brickRot;
else if(brickPos(1) < 8.5)
brickRot = rotXneg14*brickRot;*/
double brickSpeed = nextBrick.getSpeed();
brickPos(0) = brickPos(0) + ((brickSpeed) * timeDiff);
brickPos(2) = fabs(brickPos(2)) + 0.005;
brickPos(0) += BRICK_READY_X_OFFSET; // do not actually move the offset!
Transform3D<> brickTransform(brickPos, brickRot);//conveyorToBrick.R());
rw::common::Log::log().info() << "brickTransform: \n" << brickTransform << std::endl;
Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6");
MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>();
objectMovFrame->setTransform(brickTransform, _state);
Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state);
Q crapQ(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
Q brickTargetQ = moveInvKin(baseToObject, 0.036);
//moveToTarget(baseToObject, 0.05, false);//TODO -----------------HACK------------------
//moveToTarget(baseToObject, -0.03, false);
if(brickTargetQ != crapQ) {
moveToTarget(brickTargetQ);
}
else
{
linearPath.clear();
rw::common::Log::log().info() << "Brick no solution for InvKin!" << std::endl;
visionQueue.pop();
control_state = GET_TARGETS;
break;
}
if (!MANUAL_MODE) {
// If the targets queue is not empty, Issue move command for moving robot to next brick position target
target_type = BRICK;
moveRobot();
last_state = MOVING_TO_BRICK_READY;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK" << std::endl;
control_state = MOVING_TO_BRICK;
}
else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) {
// If the targets queue is not empty, Issue move command for moving robot to next brick position target
target_type = BRICK;
moveRobot();
last_state = MOVING_TO_BRICK_READY;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK" << std::endl;
control_state = MOVING_TO_BRICK;
}
}
break;
case WAITING_FOR_BRICK:
{
double currentTime;
if(conveyorHasStopped)
{
currentTime = conveyorStoppedTime;
}
else
{
currentTime = ros::Time::now().toSec();
}
Frame::Ptr camFrame = _rwWorkCell->findFrame("webCamFrame");
Transform3D<> conveyorToCam = camFrame->getTransform(_state);
conveyorToBrick = conveyorToCam*nextBrick.getBrickTransform();
double timeDiff = currentTime - nextBrick.getTimestamp();
RPY<> brickRot(conveyorToBrick.R());
Vector3D<> brickPos = conveyorToBrick.P();
double brickSpeed = nextBrick.getSpeed();
brickPos(0) = brickPos(0) + ((brickSpeed) * timeDiff);
brickPos(2) = 0.010;
if (brickPos(0) >= (END_GRASP_AREA - BRICK_READY_X_OFFSET) || conveyorHasStopped)
{
//stop conveyor belt
PLCserial.write("s");
if(!conveyorStoppedTimeSaved) {
conveyorStoppedTime = ros::Time::now().toSec();
conveyorStoppedTimeSaved = true;
}
conveyorHasStopped = true;
Frame::Ptr camFrame = _rwWorkCell->findFrame("webCamFrame");
Transform3D<> conveyorToCam = camFrame->getTransform(_state);
conveyorToBrick = conveyorToCam*nextBrick.getBrickTransform();
double currentTime=0.0;
if(conveyorHasStopped)
{
currentTime = conveyorStoppedTime;
}
double timeDiff = currentTime - nextBrick.getTimestamp();
//RPY<> brickReadyRot(conveyorToBrick.R());
for(int i=0; i<conveyorToBrick.R().m().size1(); i++)
for(int j=0; j<conveyorToBrick.R().m().size2(); j++)
if(fabs(0-conveyorToBrick.R()(i,j)) < 0.00001)
conveyorToBrick.R()(i,j) = 0.0;
Vector3D<> brickReadyPos = conveyorToBrick.P();
brickReadyPos(0) += BRICK_READY_X_OFFSET;
double brickSpeed = nextBrick.getSpeed();
brickReadyPos(0) = brickReadyPos(0) + ((brickSpeed)*timeDiff);
brickReadyPos(2) = 0.030;
rw::common::Log::log().info() << "brickReady X: \n" << brickReadyPos(0) << std::endl;
// Are we past the grasp area?
if(brickReadyPos(0) >= (END_GRASP_AREA + BRICK_READY_X_OFFSET) + 0.05) {
rw::common::Log::log().info() << "Break!!!!" << std::endl;
visionQueue.pop();
control_state = GET_TARGETS;
break;
}
Transform3D<> brickReadyTransform(brickReadyPos, conveyorToBrick.R());
Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6");
MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>();
rw::common::Log::log().info() << "brickReadyTransform: \n" << brickReadyTransform << std::endl;
objectMovFrame->setTransform(brickReadyTransform, _state);
Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state);
Q crapQ(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
brickReadyTargetQ = moveInvKin(baseToObject, -0.050);
if(brickReadyTargetQ != crapQ) {
moveToTarget(brickReadyTargetQ);
}
else
{
rw::common::Log::log().info() << "BrickReady no solution for InvKin!" << std::endl;
linearPath.clear();
visionQueue.pop();
control_state = GET_TARGETS;
break;
}
if (!MANUAL_MODE) {
target_type = BRICKREADY;
moveRobot();
last_state = WAITING_FOR_BRICK;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK_READY" << std::endl;
control_state = MOVING_TO_BRICK_READY;
}
else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT)
{
target_type = BRICKREADY;
moveRobot();
last_state = WAITING_FOR_BRICK;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BRICK_READY" << std::endl;
control_state = MOVING_TO_BRICK_READY;
}
}
}
break;
case MOVING_TO_BRICK:
{
// If brick position target is reached?
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BRICK_REACHED) {
linearPath.clear();
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BRICK_REACHED);
Q gripperCloseQ(1, 0.007); // Q gripperCloseQ(1, 0.0); without extension
sendQtoGripper(gripperCloseQ);
closeGripper();
_graspTimer->start(GRASP_TIME);
last_state = MOVING_TO_BRICK;
rw::common::Log::log().info() << "In State: " << "GRASPING" << std::endl;
control_state = GRASPING;
}
}
break;
case GRASPING:
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_GRASPING_DONE)
{
nrOfGrasps++;
// Issue move command for moving robot to box position target and switch state to MOVING_TO_BOX
if(graspSuccessful())
{
rw::common::Log::log().info() << "Grasp successful.." << std::endl;
string colorOfBrick = nextBrick.getColor();
if((colorOfBrick == "Blue") && (currentOrder.get_blueBricks() > 0))
currentOrder.decrementBrick(colorOfBrick);
else if ((colorOfBrick == "Red") && (currentOrder.get_redBricks() > 0))
currentOrder.decrementBrick(colorOfBrick);
else if ((colorOfBrick == "Yellow") && (currentOrder.get_yellowBricks() > 0))
currentOrder.decrementBrick(colorOfBrick);
else {
rw::common::Log::log().info() << "Spare brick grasped!" << std::endl;
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_SET, STATUS_SPAREBRICK_GRASPED);
}
}
else
{
rw::common::Log::log().info() << "Grasp unsuccessful!" << std::endl;
}
if(linearPath.empty())
{
moveToTarget(brickReadyTargetQ);
Q safeQJointPos, spareBoxJointPos, boxJointPos;
safeQJointPos = Q(7, 0.971, -0.988, -0.243, -0.923, -0.049, -1.515, -0.754);
moveToTarget(safeQJointPos);
if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED)
{
spareBoxJointPos = Q(7, 95.0*Deg2Rad, -20.0*Deg2Rad, 0.0*Deg2Rad, -100.0*Deg2Rad, 0.0*Deg2Rad, -60.0*Deg2Rad, 8.0*Deg2Rad);
moveToTarget(spareBoxJointPos);
}
else
{
boxJointPos = Q(7, 95.0*Deg2Rad, -90.0*Deg2Rad, 0.0*Deg2Rad, -10.0*Deg2Rad, 0.0*Deg2Rad, -80.0*Deg2Rad, 0.0*Deg2Rad);
moveToTarget(boxJointPos);
}
}
if (!MANUAL_MODE)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_GRASPING_DONE);
if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBRICK_GRASPED);
target_type = SPAREBOX;
}
else
{
target_type = BOX;
}
moveRobot();
last_state = GRASPING;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BOX" << std::endl;
control_state = MOVING_TO_BOX;
}
else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_GRASPING_DONE);
if(get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBRICK_GRASPED)
{
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBRICK_GRASPED);
target_type = SPAREBOX;
}
else
{
target_type = BOX;
}
moveRobot();
last_state = GRASPING;
rw::common::Log::log().info() << "In State: " << "MOVING_TO_BOX" << std::endl;
control_state = MOVING_TO_BOX;
}
}
break;
case MOVING_TO_BOX:
// If Box position is reached?
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_BOX_REACHED || get_rsd_info(INFO_TYPE_STATUS) & STATUS_SPAREBOX_REACHED) {
linearPath.clear();
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_BOX_REACHED);
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_SPAREBOX_REACHED);
Q gripperInitOpenQ(1, 0.020);
sendQtoGripper(gripperInitOpenQ);
//openGripper();//TODO wont open enougth
_graspTimer->start(GRASP_TIME);
target_type = NONE;
last_state = MOVING_TO_BOX;
rw::common::Log::log().info() << "In State: " << "UNGRASPING" << std::endl;
control_state = UNGRASPING;
}
break;
case UNGRASPING:
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_UNGRASPING_DONE)
{
visionQueue.pop();
if (linearPath.empty()) {
// Issue move command for moving robot to ready position and switch state to MOVING_TO_READY_POS
Q readyJointPos = Q(7, 0.972, -0.988, -0.242, -0.923, -0.049, -1.515, -0.751);
moveToTarget(readyJointPos);
}
if (!MANUAL_MODE) {
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_UNGRASPING_DONE);
target_type = READY;
moveRobot();
last_state = UNGRASPING;
control_state = MOVING_TO_READY_POS;
}
else if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) {
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_UNGRASPING_DONE);
target_type = READY;
moveRobot();
last_state = UNGRASPING;
control_state = MOVING_TO_READY_POS;
}
}
break;
case RESET:
// Clear any previous planned path, before reset
if (!(get_rsd_info(INFO_TYPE_STATUS) & (STATUS_INITIAL_REACHED)))
{
if(linearPath.empty())
{
//linearPath.clear();
Q intermediateJointPos = Q(7, 1.708, -0.988, -0.242, -0.923, 0.287, -1.235, -0.093);
moveToTarget(intermediateJointPos);
// Then do a move in joint space for the last distance
Q initJointPos = Q(7, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
moveToTarget(initJointPos);
}
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT)
{
target_type = INITIAL;
moveRobot();
control_state = MOVING_TO_RESET;
}
}
break;
case MOVING_TO_RESET:
if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_INITIAL_REACHED) {
linearPath.clear();
set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_INITIAL_REACHED);
target_type = NONE;
last_state = MOVING_TO_RESET;
rw::common::Log::log().info() << "In State: " << "ABORT" << std::endl;
control_state = ABORT;
}
break;
case ABORT:
// ABORTING!
break;
default:
break;
}
break;
case ORDER_SORTED:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_ORDERSORTED);
break;
case LOADING:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ORDERSORTED);
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_LOADING);
break;
case OUT_OF_BRICKS:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_OUTOFBRICKS);
break;
case ERROR:
set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_ERROR);
break;
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "IMU_Node2");
ros::NodeHandle n;
imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);
// Change the next line according to your port name and baud rate
try
{
if(device.isOpen())
{
ROS_INFO("Port is opened");
}
}
catch(serial::SerialException& e)
{
ROS_FATAL("Failed to open the serial port!!!");
ROS_BREAK();
}
ros::Rate r(50);
while(ros::ok())
{
// Get the reply, the last value is the timeout in ms
try{
std::string reply;
// ros::Duration(0.005).sleep();
device.readline(reply);
Roll = strtod(reply.c_str(),&pRoll)/2;
Pitch = strtod(pRoll,&pRoll)/2;
Yaw = strtod(pRoll,&pRoll)/2;
GyroX = strtod(pRoll,&pRoll);
GyroY = strtod(pRoll,&pRoll);
GyroZ = strtod(pRoll,&pRoll);
AccX = strtod(pRoll,&pRoll);
AccY = strtod(pRoll,&pRoll);
AccZ = strtod(pRoll,&pRoll);
Mx = strtod(pRoll,&pRoll);
My = strtod(pRoll,&pRoll);
Mz = strtod(pRoll,NULL);
ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);
sensor_msgs::Imu imu;
geometry_msgs::Quaternion Q;
Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);
imu.orientation.x = 0;
imu.orientation.y = 0;
imu.orientation.z = 0;
imu.orientation.w = 1;
LinearX = (AccX/256)*9.806;
LinearY = (AccY/256)*9.806;
LinearZ = (AccZ/256)*9.806;
imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;
imu.linear_acceleration.x = LinearX*-1;
imu.linear_acceleration.y = LinearY;
imu.linear_acceleration.z = LinearZ*-1;
imu.header.stamp = ros::Time::now();
imu.header.frame_id = "imu";
imu_pub.publish(imu);
}
catch(serial::SerialException& e)
{
ROS_INFO("Error %s",e.what());
}
r.sleep();
}
}