Exemplo n.º 1
0
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);

	        }
		}

	}

}
Exemplo n.º 2
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;
}
Exemplo n.º 3
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());
		}
	}
}
Exemplo n.º 4
0
        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;
        }
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
static void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
    if (chan == MAVLINK_COMM_0)
    {
    	ser.write(&ch, 1);
    }
}
Exemplo n.º 7
0
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;  
}
Exemplo n.º 8
0
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);
}
Exemplo n.º 9
0
// 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;
  }
}
Exemplo n.º 10
0
		// 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;
		}
Exemplo n.º 11
0
        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;
        }
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
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 + "]");
        }
Exemplo n.º 14
0
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;
			}
    }
  }
}
Exemplo n.º 15
0
// 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]);
}
Exemplo n.º 16
0
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;
}
Exemplo n.º 17
0
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;
}
Exemplo n.º 18
0
        bool is_open() {
				return m_serial.isOpen();
        }
Exemplo n.º 19
0
bool Move::trigger_callback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res)
{
  syncboard.write("t");
  return true;
}
Exemplo n.º 20
0
void transmit(const uint8_t* d, size_t l){
  if(!checkConnection()){
  	return;
	}
	port.write(d, l);
}
Exemplo n.º 21
0
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;
	}

}
Exemplo n.º 22
0
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();
    }

}