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(); } }