// =========================================================================
// Logging Scheduler
// ==========================================================================
void LoggingScheduler::Run() {
  // Run the txns according to the schedule
  if (!concurrent) {
    for (auto itr = sequence.begin(); itr != sequence.end(); itr++) {
      auto front_id = itr->second.front;
      auto backend_id = itr->second.back;
      PL_ASSERT(front_id != INVALID_LOGGER_IDX);

      // frontend logger's turn
      if (backend_id == INVALID_LOGGER_IDX) {
        LOG_INFO("Execute Frontend Thread %d", (int)front_id);
        frontend_threads[front_id].go = true;
        while (frontend_threads[front_id].go) {
          std::chrono::milliseconds sleep_time(1);
          std::this_thread::sleep_for(sleep_time);
        }
        LOG_INFO("Done Frontend Thread %d", (int)front_id);
      } else {
        // backend logger's turn
        LOG_INFO("Execute Backend Thread (%d, %d)", (int)front_id,
                 (int)backend_id % num_backend_logger_per_frontend);
        backend_threads[backend_id].go = true;
        while (backend_threads[backend_id].go) {
          std::chrono::milliseconds sleep_time(1);
          std::this_thread::sleep_for(sleep_time);
        }
        LOG_INFO("Done Backend Thread (%d, %d)", (int)front_id,
                 (int)backend_id % num_backend_logger_per_frontend);
      }
    }
  }
}
Ejemplo n.º 2
0
int ManageGridOptimize::svc()
{
	REPORT_THREAD_INFO();

	PlayerGridOperationVec_t player_grid_op_vec;

	ACE_Time_Value sleep_time(0, 1000);

	while (!m_is_stop)
	{
		player_grid_op_vec.clear();
		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_player_grid_op_vec_mutex, -1);
			std::copy(m_player_grid_op_vec.begin(), m_player_grid_op_vec.end(), std::back_inserter(player_grid_op_vec));
			m_player_grid_op_vec.clear();
		}

		if (player_grid_op_vec.empty())
		{
			ACE_OS::sleep(sleep_time);
			continue;
		}

		for (PlayerGridOperationVec_t::iterator it = player_grid_op_vec.begin(); it != player_grid_op_vec.end(); ++it)
		{
			PlayerGridOperation * pgo = *it;
			process(*pgo);
			delete pgo;
		}
	}

	return 0;
}
Ejemplo n.º 3
0
int PacketConvert::svc()
{
    REPORT_THREAD_INFO();

    Packet * packet = NULL;
    MSG_TYPE * protobuf_msg = NULL;
    PackInfo pack_info;
    PacketVec_t packet_vec;

    ACE_Time_Value sleep_time(0, 1 * 1000);

    while (!m_is_stop)
    {
        packet_vec.clear();
        {
            ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_packet_vec_mutex, -2);
            std::copy(m_packet_vec.begin(), m_packet_vec.end(), std::back_inserter(packet_vec));
            m_packet_vec.clear();
        }

        if (packet_vec.empty())
        {
            ACE_OS::sleep(sleep_time);
            continue;
        }

        for (PacketVec_t::iterator it = packet_vec.begin(); it != packet_vec.end(); ++it)
        {
            packet = *it;
            if (packet->opcode() == SMSG_TIMER_OCCUR)
            {
                pack_info.guid = packet->guid();
                pack_info.opcode = packet->opcode();
                pack_info.msg = NULL;
                m_scene_imp->packInput(new PackInfo(pack_info.opcode, pack_info.guid, pack_info.msg));
            }
            else
            {
                protobuf_msg = NULL;
                if (extractPacket(packet, protobuf_msg))
                {
                    pack_info.guid = packet->guid();
                    pack_info.opcode = packet->opcode();
                    pack_info.msg = protobuf_msg;
                    PackInfo * pi = new PackInfo(pack_info.opcode, pack_info.guid, pack_info.msg);
                    pi->owner = packet->getOwner();
                    m_scene_imp->packInput(pi);
                }
                else
                {
                    DEF_LOG_ERROR("failed to extract message from packet, opcode is <%d>, guid is <%s>\n", packet->opcode(), boost::lexical_cast<string>(packet->guid()).c_str());
                }
            }

            delete packet;
        }
    }

    return 0;
}
int ManageMemberSessionOutput::svc()
{
	ACE_Time_Value sleep_time(0, 1000);
	PacketVec_t	output_packet_vec;
	while (true)
	{
		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_output_packet_vec_mutex, 1);
			std::copy(m_output_packet_vec.begin(), m_output_packet_vec.end(), std::back_inserter(output_packet_vec));
			m_output_packet_vec.clear();
		}

		if (output_packet_vec.size() == 0)
		{
			ACE_OS::sleep(sleep_time);
			continue;
		}
	
		for (PacketVec_t::iterator it = output_packet_vec.begin(); it != output_packet_vec.end(); ++it)
		{
			Packet * packet = *it;
			processPacket(packet);
		}

		output_packet_vec.clear();
	}
	return 0;
}
Ejemplo n.º 5
0
void thread_sleep_v3(const tick_count::interval_t &i)
{
#if _WIN32||_WIN64
     tick_count t0 = tick_count::now();
     tick_count t1 = t0;
     for(;;) {
         double remainder = (i-(t1-t0)).seconds()*1e3;  // milliseconds remaining to sleep
         if( remainder<=0 ) break;
         DWORD t = remainder>=INFINITE ? INFINITE-1 : DWORD(remainder);
#if !__TBB_WIN8UI_SUPPORT
         Sleep( t );
#else
         std::chrono::milliseconds sleep_time( t );
         std::this_thread::sleep_for( sleep_time );
#endif
         t1 = tick_count::now();
    }
#else
    struct timespec req;
    double sec = i.seconds();

    req.tv_sec = static_cast<long>(sec);
    req.tv_nsec = static_cast<long>( (sec - req.tv_sec)*1e9 );
    nanosleep(&req, NULL);
#endif // _WIN32||_WIN64
}
Ejemplo n.º 6
0
void UserMappingGridmap::PeriodicGridmapFileReload() {
  struct stat st;
  // Monitor changes to the gridmap file.
  while (true) {
    boost::posix_time::seconds sleep_time(gridmap_reload_interval_s_);
    boost::this_thread::sleep(sleep_time);

    int ierr = stat(gridmap_file_.c_str(), &st);
    if (ierr) {
      if (Logging::log->loggingActive(LEVEL_WARN)) {
        Logging::log->getLog(LEVEL_WARN)
            << "Failed to check if the gridmap file has changed."
               " Is it temporarily not available? Path to file: "
            << gridmap_file_ << " Error: " << ierr << endl;
      }
      continue;
    }

    if (st.st_mtime != date_ || st.st_size != size_) {
      if (Logging::log->loggingActive(LEVEL_INFO)) {
        Logging::log->getLog(LEVEL_INFO)
            << "File changed. Updating all entries." << endl;
      }
      ReadGridmapFile();
      date_ = st.st_mtime;
      size_ = st.st_size;
    }
  }
}
Ejemplo n.º 7
0
int ManageLogger::svc()
{
	int wait_index = 0;
	ACE_Time_Value sleep_time(0, 1 * 1000);
	LoggerCtn_t logger_ctn;
	LoggerSet_t logger_flush_set;

	while (!m_stop)
	{
		logger_ctn.clear();

		// get the copy of logger content
		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_logger_mutex, -1);
			if (!m_logger_ctn.empty())
			{
				std::copy(m_logger_ctn.begin(), m_logger_ctn.end(), std::back_inserter(logger_ctn));
				m_logger_ctn.clear();
			}
		}

		// if logger is empty, continue
		if (logger_ctn.empty())
		{
			{
				ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_logger_flush_set_mutex, -1);
				if (m_logger_flush_set.size() > 0)
				{
					for (LoggerSet_t::iterator it = m_logger_flush_set.begin(); it != m_logger_flush_set.end(); ++it)
					{
						logger_flush_set.insert(*it);
					}
				}
			}

			if (m_need_flush)
			{
				m_need_flush = false;
				flushLogger(logger_flush_set);
			}
			else
			{
				ACE_OS::sleep(sleep_time);
			}
			continue;
		}

		m_need_flush = true;

		for (LoggerCtn_t::iterator it = logger_ctn.begin(); it != logger_ctn.end(); ++it)
		{
			doLogging(it->first, it->second);
		}
	}

	m_wait = false;

	return 0;
}
Ejemplo n.º 8
0
Archivo: util.cpp Proyecto: RRIE/to3ds
void rt_sleep(int msec) {
#if !WIN8UI_EXAMPLE
    Sleep(msec);
#else
    std::chrono::milliseconds sleep_time( msec );
    std::this_thread::sleep_for( sleep_time );
#endif
}
Ejemplo n.º 9
0
int MakeGuid::svc()
{
	typed::protocol::RequestGuid request_guid;
	request_guid.set_request_no(m_request_no);
	ACE_Time_Value timeout_v(1, 0);
	ACE_Message_Block recv_buffer(255);

	MAKE_NEW_PACKET(ps, SMSG_GUID_REQUEST, 0, request_guid.SerializeAsString());

	ACE_Time_Value sleep_time(0, 10 * 1000);
	ACE_Time_Value start_time;
	ACE_Time_Value diff_time;
	while (true)
	{
		ACE_OS::sleep(sleep_time);

		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_guid_info_que_mutex, -1);
			if (m_guid_info_que.size() > 0)
			{
				continue;
			}
		}

		start_time = ACE_OS::gettimeofday();
		int send_n = m_sock_stream.send_n(ps->stream(), ps->stream_size());
		if (send_n == ps->stream_size())
		{
			bool parsed_packet = false;
			recv_buffer.crunch();
			while (!parsed_packet)
			{
				int recv_n = m_sock_stream.recv(recv_buffer.wr_ptr(), recv_buffer.space(), &timeout_v);
				if (recv_n > 0)
				{
					recv_buffer.wr_ptr(recv_n);
					parsed_packet = parsePack(recv_buffer);
					if (parsed_packet)
					{
						diff_time = ACE_OS::gettimeofday() - start_time;
						ACE_UINT64 dt = 0;
						diff_time.to_usec(dt);
						DEF_LOG_INFO("request time is : <%s>\n", boost::lexical_cast<string>(dt).c_str());
					}
				}
				else
				{
					// error
				}
			}
		}
		else
		{
			// error
		}
	}
}
Ejemplo n.º 10
0
void Widget::on_pushButton_clicked()
{
    boost::thread worker([](){
        boost::posix_time::seconds sleep_time(3);
        boost::this_thread::sleep(sleep_time);

        qDebug() << "thread about to be finished!" << endl;
    });

    worker.join();
}
Ejemplo n.º 11
0
   bool A53IntAider(int cpu, int seq, void * data)
   {
       unsigned long n=(unsigned long)(data);

       LOG_INFO()<<"cpu: "<<cpu<<" A53 INT called\n";
       LOG_INFO()<<"cpu: "<<cpu<<" will sleep "<<n<<" seconds\n";
 
       std::chrono::milliseconds sleep_time(n*1000);
       std::this_thread::sleep_for(sleep_time);

       LOG_INFO()<<"cpu: "<<cpu<<" DONE\n";

       return true;
   }
// =======================================================================
// Abstract Logging Thread
// =======================================================================
void AbstractLoggingThread::MainLoop() {
  while (true) {
    while (!go) {
      std::chrono::milliseconds sleep_time(1);
      std::this_thread::sleep_for(sleep_time);
    };
    ExecuteNext();
    if (cur_seq == (int)schedule->operations.size()) {
      go = false;
      return;
    }
    go = false;
  }
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "subscribe_unsubscribe_repeatedly");
  ros::NodeHandle nh;
  ros::Duration sleep_time(0, 100000000);
  while(ros::ok())
  {
    sleep_time.sleep();
    ros::Subscriber sub = nh.subscribe("test_roscpp/pubsub_test", 1, callback);
    sleep_time.sleep();
  }
  
  return 0;
}
int ManageMemberSessionCooler::svc()
{
	ACE_Time_Value sleep_time(0, 10 * 1000);
	MemberSessionSet_t lost_connection_set;
	while (true)
	{
		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, m_lost_connection_set_mutex, 1);
			for (MemberSessionSet_t::iterator it = m_lost_connection_set.begin(); it != m_lost_connection_set.end(); ++it)
			{
				lost_connection_set.insert(*it);
			}
			m_lost_connection_set.clear();
		}

		uint64 curr_time = 0;
		ACE_OS::gettimeofday().to_usec(curr_time);

		{
			for (MemberSessionSet_t::iterator it = lost_connection_set.begin(); it != lost_connection_set.end(); ++it)
			{
				cleanMemberSession(*it);
				m_cleaned_connection_que.push(new CoolerInfo(*it, curr_time));
			}
			lost_connection_set.clear();
		}

		if (m_cleaned_connection_que.size() == 0)
		{
			ACE_OS::sleep(sleep_time);
			continue;
		}

		while (m_cleaned_connection_que.size() > 0)
		{
			CoolerInfo * coller_info = m_cleaned_connection_que.front();
			if ((curr_time - coller_info->close_time) >= 15)
			{
				m_cleaned_connection_que.pop();
				delete coller_info;
			}
			else
			{
				break;
			}
		}
	}
	return 0;
}
Ejemplo n.º 15
0
void TransactionTest(concurrency::TransactionManager *txn_manager) {
  uint64_t thread_id = TestingHarness::GetInstance().GetThreadId();

  for (oid_t txn_itr = 1; txn_itr <= 50; txn_itr++) {
    txn_manager->BeginTransaction();
    if (thread_id % 2 == 0) {
      std::chrono::microseconds sleep_time(1);
      std::this_thread::sleep_for(sleep_time);
    }

    if (txn_itr % 25 != 0) {
      txn_manager->CommitTransaction();
    } else {
      txn_manager->AbortTransaction();
    }
  }
}
/*----------------------------------------------------------------------
|       ShowResponse
+---------------------------------------------------------------------*/
static void
ShowResponse(NPT_HttpResponse* response)
{
    bool check_available = true;//true;
    
    // show entity
    NPT_HttpEntity* entity = response->GetEntity();
    if (entity == NULL) return;
    
    NPT_Console::OutputF("ENTITY: length=%lld, type=%s, encoding=%s\n",
                         entity->GetContentLength(),
                         entity->GetContentType().GetChars(),
                         entity->GetContentEncoding().GetChars());

    NPT_DataBuffer buffer(65536);
    NPT_Result result;
    NPT_InputStreamReference input;
    entity->GetInputStream(input);
    
    NPT_TimeStamp start;
    NPT_System::GetCurrentTimeStamp(start);
    float total_read = 0.0f;
    for (;;) {
        NPT_Size bytes_read = 0;
        NPT_LargeSize available = 0;
        NPT_Size to_read = 65536;
        if (check_available) {
            input->GetAvailable(available);
            if ((NPT_Size)available < to_read) to_read = (NPT_Size)available;
            if (to_read == 0) {
                to_read = 1;
                NPT_TimeStamp sleep_time(0.01f);
                NPT_System::Sleep(sleep_time);
            }
        }
        result = input->Read(buffer.UseData(), to_read, &bytes_read);
        if (NPT_FAILED(result)) break;
        total_read += bytes_read;
        NPT_TimeStamp now;
        NPT_System::GetCurrentTimeStamp(now);
        NPT_TimeStamp duration = now-start;
        NPT_Console::OutputF("%6d avail, read %6d bytes, %6.3f KB/s\n", (int)available, bytes_read, (float)((total_read/1024.0)/(double)duration));
    }
}
Ejemplo n.º 17
0
/**
 *  This is where we send all the test messages. Depending o the
 *  argument, either we read a static XML evnt from a file (and thus
 *  there is no XML conversion overhead incurred), or we instantiate
 *  and populate a struct Query_struct for every message and convert
 *  it to XML format using a QMS_Remos_Msg object on the fly.
 */
void
generate_query(void)
{
  QMS_Trace ("generate_query", __LINE__, __FILE__);
  DEBUG0 (DEBUG_L_ENTER, "DEBUG Thread Created\n");
  ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval * ACE_OS::rand()%30, 0);
  DEBUG0 (DEBUG_L_CALL, "DEBUG Thread sleeping\n");
  ACE_Thread::yield();
  ACE_OS::sleep (sleep_time);
  DEBUG0 (DEBUG_L_CALL, "DEBUG Thread awake\n");

  if (filename_p)
   {
     file_message(filename_p);
   }
  else
   {
     mock_message();
   }
  DEBUG0 (DEBUG_L_LEAVE, "DEBUG Thread all done, exiting\n");
}
Ejemplo n.º 18
0
int ManageTimer::svc()
{
	REPORT_THREAD_INFO();

	ACE_Time_Value sleep_time(0, 100 * 1000);
	ACE_Time_Value curr_time;
	ACE_Time_Value * curr_timeout;
	while (!m_is_stop)
	{
		if (!m_is_trigger)
		{
			ACE_OS::sleep(sleep_time);
			continue;
		}

		if (m_timer_queue->is_empty())
		{
			ACE_OS::sleep(sleep_time);
			continue;
		}

		curr_time = m_timer_queue->gettimeofday ();

		curr_timeout = m_timer_queue->calculate_timeout(&curr_time);

		if (*curr_timeout == ACE_Time_Value::zero)
		{
			m_timer_queue->expire();
		}
		else
		{
//			DEF_LOG_DEBUG("sleep time , year<%s>, \n", boost::lexical_cast<string>(next_timeout_time.sec()).c_str());
			ACE_OS::sleep(*curr_timeout);
			m_timer_queue->expire();
		}
	}
	return 0;
}
Ejemplo n.º 19
0
TEST_F(CheckpointTests, CheckpointModeTransitionTest) {
  logging::LoggingUtil::RemoveDirectory("pl_checkpoint", false);

  auto &log_manager = logging::LogManager::GetInstance();
  auto &checkpoint_manager = logging::CheckpointManager::GetInstance();
  checkpoint_manager.DestroyCheckpointers();

  checkpoint_manager.Configure(CHECKPOINT_TYPE_NORMAL, true, 1);

  // launch checkpoint thread, wait for standby mode
  auto thread = std::thread(&logging::CheckpointManager::StartStandbyMode,
                            &checkpoint_manager);

  checkpoint_manager.WaitForModeTransition(peloton::CHECKPOINT_STATUS_STANDBY,
                                           true);

  // Clean up table tile state before recovery from checkpoint
  log_manager.PrepareRecovery();

  // Do any recovery
  checkpoint_manager.StartRecoveryMode();

  // Wait for standby mode
  checkpoint_manager.WaitForModeTransition(CHECKPOINT_STATUS_DONE_RECOVERY,
                                           true);

  // Now, enter CHECKPOINTING mode
  checkpoint_manager.SetCheckpointStatus(CHECKPOINT_STATUS_CHECKPOINTING);
  auto checkpointer = checkpoint_manager.GetCheckpointer(0);
  while (checkpointer->GetCheckpointStatus() !=
         CHECKPOINT_STATUS_CHECKPOINTING) {
    std::chrono::milliseconds sleep_time(10);
    std::this_thread::sleep_for(sleep_time);
  }
  checkpoint_manager.SetCheckpointStatus(CHECKPOINT_STATUS_INVALID);
  thread.join();
}
Ejemplo n.º 20
0
int OutputSessionPool::svc()
{
	REPORT_THREAD_INFO();

	OutputSessionThreadInfo output_session_thread_info;

	registerOutputSessionThreadinfo(&output_session_thread_info);

	m_actived = true;

	CellSessionSet_t cell_session_set;

	int write_result = 1;

	ACE_Time_Value sleep_time(0, 1000);

	while (!m_stop)
	{
		{
			ACE_GUARD_RETURN(ACE_Thread_Mutex, guard, output_session_thread_info.mutex, -1);
			for (CellSessionSet_t::iterator it = output_session_thread_info.remove_cell_session_set.begin(); it != output_session_thread_info.remove_cell_session_set.end(); ++it)
			{
				CellSessionSet_t::iterator find_it = cell_session_set.find(*it);
				if (find_it != cell_session_set.end())
				{
					CellSessionx * cell = *find_it;
					cell_session_set.erase(find_it);
					m_session_pool->removeSession(cell);
				}
				else
				{
					// error
				}
			}
			output_session_thread_info.remove_cell_session_set.clear();

			for (CellSessionSet_t::iterator it = output_session_thread_info.add_cell_session_set.begin(); it != output_session_thread_info.add_cell_session_set.end(); ++it)
			{
				cell_session_set.insert(*it);
			}
			output_session_thread_info.add_cell_session_set.clear();
		}

		write_result = 1;
		for (CellSessionSet_t::iterator it = cell_session_set.begin(); it != cell_session_set.end(); )
		{
			CellSessionx * cell_session = *it;
			if (NULL == cell_session)
			{
				continue;
			}
			int wr = cell_session->wt_stream();
			if (-1 == wr)
			{
				cell_session_set.erase(it++);
			}
			else if (1 == wr)
			{
				// empty write
				++it;
			}
			else
			{
				write_result = 0;
				++it;
			}

		}

		if (1 == write_result)
		{
			// all empty write
			ACE_OS::sleep(sleep_time);
		}
	}

	m_wait = false;

	return 0;
}
Ejemplo n.º 21
0
    void sendCommad(){
      ros::Duration sleep_time(0.1);
      bool error = false;
      unsigned int num_req;
      while(nh_.ok() && ros::ok() && !error){
        num_req = getRequestNum();
        if (num_req > 0){
          KukaRequest req = getRequest();
          pop(); // TODO
          // Check option
          switch (req.opt_){
            case STOP:
              // Send Stop
              error = kuka_.stop();
              ROS_WARN_NAMED(name_, "Sending stop request to KUKA robot.");
              break;
            case PTP:
              // Send PTP command
              kuka_.setJointPosition(req.data_.ptp_goal);
              ROS_DEBUG_NAMED(name_, "Sending PTP request to KUKA robot.");
              break;
            case VEL:
              // Send PTP command
              kuka_.setOverrideSpeed(req.data_.vel);
              ROS_DEBUG_NAMED(name_, "Sending velocity override request to KUKA robot.");
              break;
            // case HOME:
            //   // Send Home 
            //   for(unsigned int i=0; i<6; i++) req.data_.ptp_goal[i]=0.0*rad2deg+offset_[i];
            //   kuka_.setJointPosition(req.data_.ptp_goal); 
            //   ROS_DEBUG_NAMED(name_, "Sending HOME request to KUKA robot.");
            //   break;            
            default:
              ROS_ERROR_NAMED(name_, "Undefined option type.");
          }
          sleep_time = ros::Duration(0.1);
        } 
        // Update joint state
        else if (num_req == 0) {
          ROS_DEBUG_NAMED(name_, "Update request to KUKA robot.");
          // Call KUKA update
          kuka_.update();
          // Reset delta position and norm values
          double norm = 0.0;
          for (unsigned int i = 0; i < 6; ++i) position_d_[i] = joint_state_.position[i];
          
          // Get joint position
          kuka_.getJointPosition(joint_state_.position);
          
          // Offset and rad convertion
          for (unsigned int i = 0; i < 6; ++i)
          {
            joint_state_.position[i] = (joint_state_.position[i] - offset_[i])*deg2rad;
            position_d_[i] -= joint_state_.position[i];
            norm += position_d_[i]*position_d_[i];
          }
          //norm = sqrt(norm);
          if ( norm < 0.01 ) sleep_time = ros::Duration(1);
          ROS_DEBUG_NAMED(name_, "Norma: %.2f", norm);

          // Publish joint state
          joint_state_.header.stamp = ros::Time::now();
          joint_state_.header.seq++;
          joint_state_pub_.publish(joint_state_);

        }
        ROS_DEBUG_THROTTLE_NAMED(0.2, name_, "Current number of req %i", getRequestNum());
        sleep_time.sleep();
      }
    }
Ejemplo n.º 22
0
//------------------------------------------------------------------------------
/// Initialize and configure the SDRAM
//------------------------------------------------------------------------------
void BOARD_ConfigureSdram(unsigned char busWidth)
{
    static const Pin pinsSdram[] = {PINS_SDRAM};
    volatile unsigned int *pSdram = (unsigned int *) AT91C_EBI_SDRAM;
    unsigned short sdrc_dbw = 0;

    switch (busWidth) {
        case 16:
            sdrc_dbw = AT91C_B16MODE_16_BITS;
            break;

        case 32:
        default:
            sdrc_dbw = AT91C_B16MODE_32_BITS;
            break;

    }

    // Enable corresponding PIOs
    PIO_Configure(pinsSdram, 1);
    
    WRITE(AT91C_BASE_SDDRC, SDDRC_MDR , AT91C_MD_SDR_SDRAM 
                                        | sdrc_dbw);    // SDRAM type 3.3V
                                        
    WRITE(AT91C_BASE_SDDRC, SDDRC_CR  , AT91C_NC_DDR10_SDR9 
                                        | AT91C_NR_13 
                                        | AT91C_CAS_3);    // row = 13, column = 9 SDRAM CAS = 3
    
    WRITE(AT91C_BASE_SDDRC, SDDRC_LPR , 0x00000000);    // Low power register => Low-power is inhibited // No define

    sleep_time(50000);               // --------- WAIT ---------

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_NOP_CMD);  // NOP command
    pSdram[0] = 0x00000000;          // Dummy read to access SDRAM : validate preceeding command
    
    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_NOP_CMD);  // NOP command
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command
    
    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_NOP_CMD);  // NOP command
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_PRCGALL_CMD);  // Precharge All Banks command
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command

    sleep_time(50000);               // --------- WAIT ---------

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_RFSH_CMD);  // AutoRefresh command
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command
       
    sleep_time(50000);               // --------- WAIT ---------

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_RFSH_CMD);  // AutoRefresh command
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command
    
    sleep_time(50000);               // --------- WAIT ---------

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_LMR_CMD);  // Set MR JEDEC compliant : Load mode Register command
    pSdram[19] = 0x5a5a5b5b;    // Perform LMR burst=1, lat=2

    WRITE(AT91C_BASE_SDDRC, SDDRC_MR, AT91C_MODE_NORMAL_CMD);  // Set Normal mode : Any access to the DDRSDRAMC is decoded normally
    pSdram[0] = 0x00000000;  // Dummy read to access SDRAM : validate preceeding command 

    WRITE(AT91C_BASE_SDDRC, SDDRC_RTR, 781);         // Set Refresh Timer (ex: ((64 x 10^-3)/8192) x 100 x 10^6 ) => 781 for 100 MHz
                                     
    WRITE(AT91C_BASE_SDDRC, SDDRC_HS, AT91C_OVL);        // High speed register : Optimization is disabled
       
    sleep_time(50000);               // --------- WAIT ---------
}
Ejemplo n.º 23
0
int main(int argc, char **argv)
{
	ros::init (argc, argv, "move_group_tutorial");
	ros::AsyncSpinner spinner(1);
	spinner.start();
	ros::NodeHandle node_handle("motion_planner_api");

	robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
	robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

	planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

	boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
	planning_interface::PlannerManagerPtr planner_instance;
	std::string planner_plugin_name;

	if (!node_handle.getParam("/move_group/planning_plugin", planner_plugin_name))
	ROS_FATAL_STREAM("Could not find planner plugin name");
	try
	{
		planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
	}
	catch(pluginlib::PluginlibException& ex)
	{
		ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
	}
	try
	{
		planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
		if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
		ROS_FATAL_STREAM("Could not initialize planner instance");
		ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
	}
	catch(pluginlib::PluginlibException& ex)
	{
		const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
		std::stringstream ss;
		for (std::size_t i = 0 ; i < classes.size() ; ++i)
		ss << classes[i] << " ";
		ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
	}

	ros::WallDuration sleep_time(15.0);
	sleep_time.sleep(); 

	planning_interface::MotionPlanRequest req;
	planning_interface::MotionPlanResponse res;

	geometry_msgs::PoseStamped pose;
	
	pose.header.frame_id = "odom";
	pose.pose.position.x = -0.0119214;
	pose.pose.position.y = 0.0972478;
	pose.pose.position.z = 0.636616;
	pose.pose.orientation.x = 0.273055;
	pose.pose.orientation.y = -0.891262;
	pose.pose.orientation.z = -0.346185;
	pose.pose.orientation.w = 0.106058;

	group.setPoseTarget(target_pose1);

	moveit::planning_interface::MoveGroup::Plan my_plan;
	bool success = group.plan(my_plan);

	if (1)
	{
		ROS_INFO("Visualizing plan 1 (again)");
		display_trajectory.trajectory_start = my_plan.start_state_;
		display_trajectory.trajectory.push_back(my_plan.trajectory_);
		display_publisher.publish(display_trajectory);
		/* Sleep to give Rviz time to visualize the plan. */
		sleep(5.0);
	}

	std::vector<double> group_variable_values;
	group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);

	group_variable_values[4] = -1.0;
	group.setJointValueTarget(group_variable_values);
	success = group.plan(my_plan);

	ROS_INFO("Visualizing plan 2 (joint space goal) %s",success?"":"FAILED");

	sleep(5.0);

	

	return 0;
}
Ejemplo n.º 24
0
int main(int argc, char **argv)
{     
      /*Initialise Variables*/
      
      CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle
      //Creating the joint_msg_leap
      joint_msg_leap.name.resize(8);
      joint_msg_leap.position.resize(8);
      joint_msg_leap.name[0]="arm_1_joint";
      joint_msg_leap.name[1]="arm_2_joint";
      joint_msg_leap.name[2]="arm_3_joint";
      joint_msg_leap.name[3]="arm_4_joint";
      joint_msg_leap.name[4]="arm_5_joint";
      joint_msg_leap.name[5]="arm_6_joint";
      joint_msg_leap.name[6]="base_joint_gripper_left";
      joint_msg_leap.name[7]="base_joint_gripper_right";
      aux_enter=1;
      FIRST_VALUE=true;//Help knowing Initial Position of the hand
      int arm_trajectory_point=1;
      
      collision_detection::CollisionRequest collision_request;
      collision_detection::CollisionResult collision_result;
      
      /*Finish Variables Initialitation*/

      //ROS DECLARATION
      ros::init(argc, argv,"listener");
      if (argc != 2)
      {
        ROS_WARN("WARNING: you should specify number of points to capture");
      }
      else
      {
        count=atoi(argv[1]);
        ROS_INFO ("Number of points /n%d", count);
      }
      
      //Create an object of the LeapMotionListener Class
      LeapMotionListener leapmotionlistener;
      leapmotionlistener.Configure(count);
      
      //ros::NodeHandle node_handle;
      ros::NodeHandle node_handle("~");
      // start a ROS spinning thread
      ros::AsyncSpinner spinner(1);
      spinner.start();
      //we need this for leap
      ros::Rate r(1);
      //robo_pub = n.advertise<sensor_msgs::JointState>("joint_leap", 100);
      
      //Creating a Robot Model
      robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
      robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
      //Initialise PlanningSceneMonitorPtr

      
      /* MOVEIT Setup*/
      // ^^^^^
      moveit::planning_interface::MoveGroup group("arm");
      group.setPlanningTime(0.5);
      moveit::planning_interface::MoveGroup::Plan my_plan;
      // We will use the :planning_scene_interface:`PlanningSceneInterface`
      // class to deal directly with the world.
      moveit::planning_interface::PlanningSceneInterface planning_scene_interface;  
      
      /* Adding/Removing Objects and Attaching/Detaching Objects*/
      ROS_INFO("CREATING PLANNING_SCENE PUBLISHER");
      ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
      while(planning_scene_diff_publisher.getNumSubscribers() < 1)
      {
        ros::WallDuration sleep_t(0.5);
        sleep_t.sleep();
      }
      ROS_INFO("CREATING COLLISION OBJECT");
      
      moveit_msgs::AttachedCollisionObject attached_object;
      attached_object.link_name = "";
      /* The header must contain a valid TF frame*/
      attached_object.object.header.frame_id = "gripper_left";
      /* The id of the object */
      attached_object.object.id = "box";

      /* A default pose */
      geometry_msgs::Pose posebox;
      posebox.orientation.w = 1.0;

      /* Define a box to be attached */
      shape_msgs::SolidPrimitive primitive;
      primitive.type = primitive.BOX;
      primitive.dimensions.resize(3);
      primitive.dimensions[0] = 0.1;
      primitive.dimensions[1] = 0.1;
      primitive.dimensions[2] = 0.1;

      attached_object.object.primitives.push_back(primitive);
      attached_object.object.primitive_poses.push_back(posebox);
      attached_object.object.operation = attached_object.object.ADD;

      
      ROS_INFO("ADDING COLLISION OBJECT TO THE WORLD");
      
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(attached_object.object);
planning_scene_msg.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();

/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;

/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene_msg.world.collision_objects.clear();
planning_scene_msg.world.collision_objects.push_back(remove_object);
planning_scene_msg.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene_msg);

//sleep_time.sleep();
     
      // Create a publisher for visualizing plans in Rviz.
      ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
      planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model,collision_detection::WorldPtr(new collision_detection::World())));
      
      //Creating planning_scene_monitor
      boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(2.0)));
      planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(planning_scene,"robot_description", tf));
      planning_scene_monitor->startSceneMonitor();
      //planning_scene_monitor->initialize(planning_scene);
      planning_pipeline::PlanningPipeline *planning_pipeline= new planning_pipeline::PlanningPipeline(robot_model,node_handle,"planning_plugin", "request_adapters");

      /* Sleep a little to allow time to startup rviz, etc. */
      ros::WallDuration sleep_time(20.0);
      sleep_time.sleep();  
      /*end of MOVEIT Setup*/

      // We can print the name of the reference frame for this robot.
      ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
      // We can also print the name of the end-effector link for this group.
      ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
      
      // Planning to a Pose goal 1
      // ^^^^^^^^^^^^^^^^^^^^^^^
      // We can plan a motion for this group to a desired pose for the 
      // end-effector  
      ROS_INFO("Planning to INITIAL POSE");
      planning_interface::MotionPlanRequest req;
      planning_interface::MotionPlanResponse res;
      geometry_msgs::PoseStamped pose;
      pose.header.frame_id = "/odom_combined";
      pose.pose.position.x = 0;
      pose.pose.position.y = 0;
      pose.pose.position.z = 1.15;
      pose.pose.orientation.w = 1.0;
      std::vector<double> tolerance_pose(3, 0.01);
      std::vector<double> tolerance_angle(3, 0.01);
      old_pose=pose;
      
      // We will create the request as a constraint using a helper function available
      req.group_name = "arm";
      moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
      req.goal_constraints.push_back(pose_goal);
      // Now, call the pipeline and check whether planning was successful.
      planning_pipeline->generatePlan(planning_scene, req, res);
      /* Check that the planning was successful */
      if(res.error_code_.val != res.error_code_.SUCCESS)
      {
      ROS_ERROR("Could not compute plan successfully");
      return 0;
      }
      // Visualize the result
      // ^^^^^^^^^^^^^^^^^^^^
      /* Visualize the trajectory */
      moveit_msgs::DisplayTrajectory display_trajectory;
      ROS_INFO("Visualizing the trajectory 1");
      moveit_msgs::MotionPlanResponse response;
      res.getMessage(response);
      display_trajectory.trajectory_start = response.trajectory_start;
      display_trajectory.trajectory.push_back(response.trajectory);
      display_publisher.publish(display_trajectory);
      //sleep_time.sleep();
      /* End Planning to a Pose goal 1*/

     // First, set the state in the planning scene to the final state of the last plan 
      robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
      //planning_scene->setCurrentState(response.trajectory_start);
      joint_model_group = robot_state.getJointModelGroup("arm");
      robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
      spinner.stop();
      
      /*Capturing Stage*/
      /*****************/
      ROS_INFO("PRESH ENTER TO START CAPTURING POINTS");
      while (getline(std::cin,s))
      {
        if ('\n' == getchar())
          break;
      }
      
      /* SENSOR SUBSCRIBING */
      //LEAP MOTION
      ROS_INFO("SUBSCRIBING LEAPMOTION");
      ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
      ros::Subscriber trajectorysub = node_handle.subscribe("/move_group/", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
      while(!CAPTURE_MOVEMENT==true)
      {
       
       ros::spinOnce();
       
      }
      leapsub.shutdown();
      ROS_INFO("CAPTURING POINTS FINISH...PROCESSING POINTS");
      // End of Capturing Stage
      

      /* Start Creating Arm Trajectory*/
      /**********************************/
      
      ROS_INFO("START CREATING ARM TRAJECTORY");
      for (unsigned i=0; i<trajectory_hand.size(); i++)
      {
        //First we set the initial Position of the Hand
        if (FIRST_VALUE)
        {
        dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
        dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
        dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
        FIRST_VALUE=0;
        ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n  Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
        sleep(2);
        }
        else
        {
        // Both limits for x,y,z to avoid small changes
        Updifferencex=dataLastHand_.palmpos.x+10;
        Downdifferencex=dataLastHand_.palmpos.x-10;
        Updifferencez=dataLastHand_.palmpos.z+10;
        Downdifferencez=dataLastHand_.palmpos.z-20;
        Updifferencey=dataLastHand_.palmpos.y+20;
        Downdifferencey=dataLastHand_.palmpos.y-20;
        if ((trajectory_hand.at(i).palmpos.x<Downdifferencex)||(trajectory_hand.at(i).palmpos.x>Updifferencex)||(trajectory_hand.at(i).palmpos.y<Downdifferencey)||(trajectory_hand.at(i).palmpos.y>Updifferencey)||(trajectory_hand.at(i).palmpos.z<Downdifferencez)||(trajectory_hand.at(i).palmpos.z>Updifferencez))
          {
            
            ros::AsyncSpinner spinner(1);
            spinner.start();
            ROS_INFO("TRYING TO ADD POINT %d TO TRAJECTORY",arm_trajectory_point);
            // Cartesian Paths
            // ^^^^^^^^^^^^^^^
            // You can plan a cartesian path directly by specifying a list of waypoints
            // for the end-effector to go through. Note that we are starting
            // from the new start state above. The initial pose (start state) does not
            // need to be added to the waypoint list.
            pose.header.frame_id = "/odom_combined";
            pose.pose.orientation.w = 1.0;
            pose.pose.position.y +=(trajectory_hand.at(i).palmpos.x-dataLastHand_.palmpos.x)/500 ;
            pose.pose.position.z +=(trajectory_hand.at(i).palmpos.y-dataLastHand_.palmpos.y)/1000 ;
            if(pose.pose.position.z>Uplimitez)
            pose.pose.position.z=Uplimitez;
            pose.pose.position.x +=-(trajectory_hand.at(i).palmpos.z-dataLastHand_.palmpos.z)/500 ;
            ROS_INFO("END EFFECTOR POSITION \n X: %f\n  Y: %f\n Z: %f\n", pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
            ROS_INFO("Palmpos \n X: %f\n  Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
            // We will create the request as a constraint using a helper function available
            ROS_INFO("1");
            pose_goal= kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
            ROS_INFO("2");
            req.goal_constraints.push_back(pose_goal);
            
            // Now, call the pipeline and check whether planning was successful.
            planning_pipeline->generatePlan(planning_scene, req, res);
            ROS_INFO("3");
            if(res.error_code_.val != res.error_code_.SUCCESS)
            {
            ROS_ERROR("Could not compute plan successfully");
            pose=old_pose;
            }
            else
            {
            // Now when we plan a trajectory it will avoid the obstacle
            
            
            res.getMessage(response);
            
            collision_result.clear();
            collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
            robot_state::RobotState copied_state = planning_scene->getCurrentState();
            planning_scene->checkCollision(collision_request, collision_result, copied_state, acm);
            ROS_INFO_STREAM("Collision Test "<< (collision_result.collision ? "in" : "not in")<< " collision");
            arm_trajectory_point++;
            // Visualize the trajectory 
            ROS_INFO("VISUALIZING NEW POINT");
            //req.goal_constraints.clear();
            display_trajectory.trajectory_start = response.trajectory_start;
            ROS_INFO("AXIS 1 NEXT TRAJECTORY IS %f\n",response.trajectory_start.joint_state.position[1] );
            display_trajectory.trajectory.push_back(response.trajectory); 
            // Now you should see two planned trajectories in series
            display_publisher.publish(display_trajectory);
            planning_scene->setCurrentState(response.trajectory_start);
            if (planning_scene_monitor->updatesScene(planning_scene))
            {
            ROS_INFO("CHANGING STATE");
            planning_scene_monitor->updateSceneWithCurrentState();
            }
            else
            {
            ROS_ERROR("NOT POSSIBLE TO CHANGE THE SCENE");
            }
            robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
            req.goal_constraints.clear();
            old_pose=pose;
            dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
            dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
            dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
            }
            //sleep(2);
            spinner.stop();     
          }
          
        }
      
      }  
   //ros::Subscriber myogestsub = n.subscribe("/myo_gest", 1000, myogestCallback);
        
      return 0;
}
Ejemplo n.º 25
0
int main(int argc, char **argv)
{
	int motion = 0;
    std::string initialpath = "";
	if (argc >= 2)
	{
		motion = atoi(argv[1]);
        if(argc >=3)
        {
            initialpath = argv[2];
        }
	}
	printf("%d Motion %d\n", argc, motion);

	ros::init(argc, argv, "move_itomp");
	ros::AsyncSpinner spinner(1);
	spinner.start();
	ros::NodeHandle node_handle("~");

	robot_model_loader::RobotModelLoader robot_model_loader(
			"robot_description");
	robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

	planning_scene::PlanningScenePtr planning_scene(
			new planning_scene::PlanningScene(robot_model));

	boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
	planning_interface::PlannerManagerPtr planner_instance;
	std::string planner_plugin_name;

	if (!node_handle.getParam("planning_plugin", planner_plugin_name))
		ROS_FATAL_STREAM("Could not find planner plugin name");
	try
	{
		planner_plugin_loader.reset(
				new pluginlib::ClassLoader<planning_interface::PlannerManager>(
						"moveit_core", "planning_interface::PlannerManager"));
	} catch (pluginlib::PluginlibException& ex)
	{
		ROS_FATAL_STREAM(
				"Exception while creating planning plugin loader " << ex.what());
	}
	try
	{
		planner_instance.reset(
				planner_plugin_loader->createUnmanagedInstance(
						planner_plugin_name));
		if (!planner_instance->initialize(robot_model,
				node_handle.getNamespace()))
			ROS_FATAL_STREAM("Could not initialize planner instance");
		ROS_INFO_STREAM(
				"Using planning interface '" << planner_instance->getDescription() << "'");
	} catch (pluginlib::PluginlibException& ex)
	{
		const std::vector<std::string> &classes =
				planner_plugin_loader->getDeclaredClasses();
		std::stringstream ss;
		for (std::size_t i = 0; i < classes.size(); ++i)
			ss << classes[i] << " ";
		ROS_ERROR_STREAM(
				"Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str());
	}

	loadStaticScene(node_handle, planning_scene, robot_model);

	/* Sleep a little to allow time to startup rviz, etc. */
	ros::WallDuration sleep_time(1.0);
	sleep_time.sleep();

	renderStaticScene(node_handle, planning_scene, robot_model);

	// We will now create a motion plan request
	// specifying the desired pose of the end-effector as input.
	planning_interface::MotionPlanRequest req;
	planning_interface::MotionPlanResponse res;

	std::vector<robot_state::RobotState> robot_states;

	int state_index = 0;

	robot_states.push_back(planning_scene->getCurrentStateNonConst());
    robot_states.push_back(robot_states.back());
    Eigen::VectorXd start_trans, goal_trans;

	// set trajectory constraints
    std::vector<Eigen::VectorXd> waypoints;
    std::vector<std::string> hierarchy;
	// internal waypoints between start and goal

    if(initialpath.empty())
    {
        hierarchy.push_back("base_prismatic_joint_x");
        hierarchy.push_back("base_prismatic_joint_y");
        hierarchy.push_back("base_prismatic_joint_z");
        hierarchy.push_back("base_revolute_joint_z");
        hierarchy.push_back("base_revolute_joint_y");
        hierarchy.push_back("base_revolute_joint_x");
        Eigen::VectorXd vec1;
        start_trans = Eigen::VectorXd(6); start_trans << 0.0, 1.0, 0.0,0,0,0;
        goal_trans =  Eigen::VectorXd(6); goal_trans << 0.0, 2.5, 0.0,0,0,0;
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 1.5, 1.0,0,0,0;
        waypoints.push_back(vec1);
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.0, 2.0,0,0,0;
        waypoints.push_back(vec1);
        vec1 = Eigen::VectorXd(6); vec1 << 0.0, 2.5, 1.0,0,0,0;
        waypoints.push_back(vec1);
    }
    else
    {
        hierarchy = InitTrajectoryFromFile(waypoints, initialpath);
        start_trans = waypoints.front();
        goal_trans =  waypoints.back();
    }
    setWalkingStates(robot_states[state_index], robot_states[state_index + 1],
            start_trans, goal_trans, hierarchy);
	for (int i = 0; i < waypoints.size(); ++i)
	{
		moveit_msgs::Constraints configuration_constraint =
                setRootJointConstraint(hierarchy, waypoints[i]);
        req.trajectory_constraints.constraints.push_back(
				configuration_constraint);
	}

	displayStates(robot_states[state_index], robot_states[state_index + 1],
				node_handle, robot_model);

	doPlan("whole_body", req, res, robot_states[state_index],
			robot_states[state_index + 1], planning_scene, planner_instance);


	visualizeResult(res, node_handle, 0, 1.0);

	ROS_INFO("Done");
	planner_instance.reset();

	return 0;
}
Ejemplo n.º 26
0
int main(int argc, char **argv)
{
  ros::init(argc, argv, "task_queue_client_ltl");
  ros::NodeHandle node_handle;
  
  ros::AsyncSpinner spinner(1);
  spinner.start();
  sleep(10.0);
  
  ros::ServiceClient client_a = node_handle.serviceClient<pr2_moveit_ltl::automaton_gen_msg>("automaton_gen_ltl");
  ros::ServiceClient client_p = node_handle.serviceClient<pr2_moveit_ltl::task_queue_msg>("plan_gen_ltl");
  
  pr2_moveit_ltl::automaton_gen_msg srv_auto;
  srv_auto.request.client = "auto_gen_client_ltl";
  
  pr2_moveit_ltl::automaton automata;
  if (client_a.call(srv_auto))
  {
  	automata = srv_auto.response.automata; 
	  std::cout << "Automata:" << automata.name << std::endl;
  }
  else
  {
    ROS_ERROR("Failed to call service plan_gen_ltl");
    return 1;
  }
  
  pr2_moveit_ltl::task_queue_msg srv;
  srv.request.name = "task_queue_client_ltl";
  srv.request.automata = automata;
  
  pr2_moveit_ltl::proposition props[10];
  bool plan = false;
  int size = 0;
  if (client_p.call(srv))
  {
    size = srv.response.size;
    int i = 0;
    for(std::vector<pr2_moveit_ltl::proposition>::const_iterator it = srv.response.propositions.begin(); it != srv.response.propositions.end(); ++it)
	{
		props[i] = *it;
		i++;
	}
	ROS_INFO("service plan_gen_ltl on");
	plan = true;
	  
  }
  else
  {
    ROS_ERROR("Failed to call service plan_gen_ltl");
    return 1;
  }
  
  //Para planear
  moveit::planning_interface::MoveGroup group("base");
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  //Collision objects
  ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
  while(collision_object_publisher.getNumSubscribers() < 1)
  {
    ros::WallDuration sleep_t(0.5);
    sleep_t.sleep();
  }  
  
  //Agregar objeto que colisiona
  moveit_msgs::CollisionObject co;
  co.header.frame_id = group.getPlanningFrame();
  co.id= "muros";

  shapes::Mesh* m = shapes::createMeshFromResource("package://pr2_moveit_ltl/models/ENV_6.dae");
  shape_msgs::Mesh co_mesh;
  shapes::ShapeMsg co_mesh_msg;
  shapes::constructMsgFromShape(m,co_mesh_msg);
  co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
  co.meshes.resize(1);
  co.meshes[0] = co_mesh;
  co.mesh_poses.resize(1);
  co.mesh_poses[0].position.x = -5.0;
  co.mesh_poses[0].position.y = -5.0;
  co.mesh_poses[0].position.z = 0.0;
  co.mesh_poses[0].orientation.w= 0.7071;
  co.mesh_poses[0].orientation.x= 0.7071 ;
  co.mesh_poses[0].orientation.y= 0.0;
  co.mesh_poses[0].orientation.z= 0.0;
  
  co.meshes.push_back(co_mesh);
  co.mesh_poses.push_back(co.mesh_poses[0]);
  co.operation = co.ADD;
  
  /* Publish and sleep (to view the visualized results) */
  collision_object_publisher.publish(co);
  ros::WallDuration sleep_time(10.0);
  sleep_time.sleep();

  std::vector<moveit_msgs::CollisionObject> collision_objects;  
  collision_objects.push_back(co);
  planning_scene_interface.addCollisionObjects(collision_objects);
  sleep(10.0);
  
  /*
  * Para planear
  */
  
  if(plan){
		for(int j = 0; j < size; j++){
			 // First get the current set of joint values for the group.
  		std::vector<double> group_variable_values;
  		group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
  
  		// Now, let's modify one of the joints, plan to the new joint
  		// space goal and visualize the plan.
  		group_variable_values[0] = props[j].point.x; 
  		group_variable_values[1] = props[j].point.y;
  		group_variable_values[2] = props[j].point.z;
  		group.setJointValueTarget(group_variable_values);

  		moveit::planning_interface::MoveGroup::Plan my_plan;
  		group.setPlannerId("RRTstarkConfigDefault");
  		group.setPlanningTime(20.0);
  		bool success = group.plan(my_plan);
  		//Mover el robot
  		if (success){
  		
    		group.move();
  		}
	    sleep(10.0);
		}
  }
  ros::spin();
  return 0;
}
Ejemplo n.º 27
0
/**
 *  @brief The workhorse routine
 *  @param argc The count of the command line arguments
 *  @param argv A pointer to the list of command line arguments
 *  @param ACE_TRY_ENV The CORBA Env variable
 *  @exception CORBA::SystemException
 *
 * This is where the bulk of the work gets done. The sequence of steps is
 *   - The orb is initialized, using the command line options, if any
 *   - The portable Object Adapter is next
 *   - Then we create a POA manager
 *   - The naming service handle is the next
 *   - After the naming context, we get a scheduler handle
 *   - Finally, we get hold of the event channel
 *
 * An overloaded variant exists that provides a similar functionality,
 * except that the event channel handle is deferred until later.
 */
void
STDC::QMS::CORBA_Handles::init (int argc, char **argv, 
				const char * orb_name_p,
				CORBA_Environment &ACE_TRY_ENV)
{
  QMS_Trace ("STDC::QMS::CORBA_Handles::init", __LINE__, __FILE__);
  DEBUG0 (DEBUG_L_ENTER, "DEBUG: QMS  API object initializing\n");
  if(this->_initialized)
   {
     return;
   } /* end of if(this->_initialized) */
  
  ACE_TRY
    {
      // ORB initialization. We shall be using the string QMS_API
      // all through this application
      this->_orb =
	CORBA::ORB_init (argc, argv, orb_name_p, ACE_TRY_ENV);
      ACE_TRY_CHECK;
      if (CORBA::is_nil (this->_orb.in ())){
	ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval, 0);
	int count = 0;
	while (CORBA::is_nil (this->_orb.in ())){
	  if (count >= QMS_DEFAULT_number_of_retries){
	    ACE_ERROR ((LM_ERROR,"(%P|%t) Unable to initialize the orb.\n"));
	    ACE_TRY_THROW (CORBA::BAD_PARAM ());
	  }
	  ACE_OS::sleep (sleep_time);
	  DEBUG0 (6, "retrying getting the orb\n");
	  this->_orb =
	    CORBA::ORB_init (argc, argv, "QMS_API", ACE_TRY_ENV);
	  ACE_TRY_CHECK;
	  count++;
	}
      }
      else
       {
	 DEBUG0 (6, "Orb initilized(no external data yet)\n");
       }

      // Ok. The portable Object Adapter is next
      if (CORBA::is_nil (this->_root_poa.in ())){
	CORBA::Object_var poa_object =
	  this->_orb->resolve_initial_references("RootPOA");
	ACE_TRY_CHECK;

	this->_root_poa =
	  PortableServer::POA::_narrow (poa_object.in (), ACE_TRY_ENV);
	ACE_TRY_CHECK;
	DEBUG0 (6, "Narrowed the root poa\n");

	if (CORBA::is_nil (poa_object.in ())){
	  ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval, 0);
	  int count = 0;
	  while (CORBA::is_nil (this->_root_poa.in ())){
	    if (count >= QMS_DEFAULT_number_of_retries){
	      ACE_ERROR ((LM_ERROR," Unable to initialize the POA.\n"));
	      ACE_TRY_THROW (CORBA::BAD_PARAM ());
	    }
	    ACE_OS::sleep (sleep_time);
	    DEBUG0 (6, "retrying getting root poa\n");

	    CORBA::Object_var poa_object =
	      this->_orb->resolve_initial_references("RootPOA");
	    ACE_TRY_CHECK;

	    this->_root_poa =
	      PortableServer::POA::_narrow (poa_object.in (), ACE_TRY_ENV);
	    ACE_TRY_CHECK;
	    DEBUG0 (6, "Narrowed the root poa\n");
	    
	    count++;
	  }
	}
	else {
	  DEBUG0 (6, "Got the root poa(no external data yet)\n");
	}
      }
      else
       {
	 DEBUG0 (6, "Already had root poa handle\n");
       }

      // Now we get the manager
      if (CORBA::is_nil (this->_poa_manager.in ())) {
	this->_poa_manager =
	  this->_root_poa->the_POAManager (ACE_TRY_ENV);
	ACE_TRY_CHECK;
	if (CORBA::is_nil (this->_poa_manager.in ())) {
	  ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval, 0);
	  int count = 0;
	  while (CORBA::is_nil (this->_poa_manager.in ())){
	    if (count >= QMS_DEFAULT_number_of_retries){
	      ACE_ERROR ((LM_ERROR,"(%P|%t) Unable to get the POA mgr.\n"));
	      ACE_TRY_THROW (CORBA::BAD_PARAM ());
	    }
	    ACE_OS::sleep (sleep_time);
	    DEBUG0 (6, "retrying getting the poa manager\n");

	    this->_poa_manager =
	      this->_root_poa->the_POAManager (ACE_TRY_ENV);
	    ACE_TRY_CHECK;

	    count++;
	  }
	}
	else {
	  DEBUG0(6, "Got the poa manager(no external data yet)\n");
	}
      }
      else
       {
	 DEBUG0 (6, "Already had poa manager handle\n");
       }

      // Obtain a reference to the naming service...
      if (CORBA::is_nil (this->_naming_context.in ())) {
	CORBA::Object_var naming_obj =
	  this->_orb->resolve_initial_references ("NameService", ACE_TRY_ENV);
	ACE_TRY_CHECK;
	this->_naming_context =
	  CosNaming::NamingContext::_narrow (naming_obj.in (), ACE_TRY_ENV);
	ACE_TRY_CHECK;
	DEBUG0 (6, "Narrowed the naming context(Connected)\n");

	if (CORBA::is_nil (this->_naming_context.in())) {
	  ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval, 0);
	  int count = 0;
	  while (CORBA::is_nil (this->_naming_context.in ())){
	    if (count >= QMS_DEFAULT_number_of_retries){
	      ACE_ERROR ((LM_ERROR,
			  "(%P|%t) Unable to get the naming context.\n"));
	      ACE_TRY_THROW (CORBA::BAD_PARAM ());
	    }
	    ACE_OS::sleep (sleep_time);
	    DEBUG0 (6, "retrying getting naming context\n");
	    CORBA::Object_var naming_obj =
	      this->_orb->resolve_initial_references ("NameService",
						      ACE_TRY_ENV);
	    ACE_TRY_CHECK;
	    this->_naming_context =
	      CosNaming::NamingContext::_narrow (naming_obj.in (), 
						 ACE_TRY_ENV);
	    ACE_TRY_CHECK;
	    DEBUG0 (6, "Narrowed the naming context(Connected)\n");
	    count++;
	  }
	}
	else {
	  DEBUG0 (6, "Got the naming context(Connected)\n");
	}
      }
      else
       {
	 DEBUG0 (6, "Already Had naming context(Connected)\n");
       }

      // and now for the scheduling service
      if  (CORBA::is_nil (this->_scheduler.in ())) {
	CosNaming::Name schedule_name (1);
	schedule_name.length (1);
	schedule_name[0].id = CORBA::string_dup ("ScheduleService");

	CORBA::Object_var sched_obj=
	  this->_naming_context->resolve (schedule_name, ACE_TRY_ENV);
	ACE_TRY_CHECK;

	this->_scheduler =
	  RtecScheduler::Scheduler::_narrow(sched_obj.in (), ACE_TRY_ENV);
	ACE_TRY_CHECK;
	DEBUG0 (6, "Narrowed  the scheduler handle\n");

	if (CORBA::is_nil (this->_scheduler.in ())) {
	  ACE_Time_Value sleep_time (QMS_DEFAULT_retry_sleep_interval, 0);
	  int count = 0;
	  while (CORBA::is_nil (this->_scheduler.in ())){
	    if (count >= QMS_DEFAULT_number_of_retries){
	      ACE_ERROR ((LM_ERROR,"(%P|%t) Unable to get the scheduler.\n"));
	      ACE_TRY_THROW (CORBA::BAD_PARAM ());
	    }
	    ACE_OS::sleep (sleep_time);
	    DEBUG0 (6, "retrying getting scheduler\n");
	    CORBA::Object_var sched_obj=
	      this->_naming_context->resolve (schedule_name, ACE_TRY_ENV);
	    ACE_TRY_CHECK;

	    this->_scheduler =
	      RtecScheduler::Scheduler::_narrow(sched_obj.in (), ACE_TRY_ENV);
	    ACE_TRY_CHECK;
	    DEBUG0 (6, "Narrowed  the scheduler handle\n");
	    count++;
	  }
	}
	else {
	  DEBUG0 (6, "Got the scheduler\n");
	}
      }
      else
       {
	 DEBUG0 (6, "Already Had scheduler\n");
       }
    }

  ACE_CATCH (CosNaming::NamingContext::AlreadyBound, ex)
    {
      ACE_PRINT_EXCEPTION (ex, "name service already bound :");
      this->disconnect();
      ACE_RE_THROW;
    }
Ejemplo n.º 28
0
int main(int argc, char **argv)
{
  ros::init (argc, argv, "move_group_tutorial");
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::NodeHandle node_handle("move_group");

  // BEGIN_TUTORIAL
  // Start
  // ^^^^^
  // Setting up to start using a planner is pretty easy. Planners are 
  // setup as plugins in MoveIt! and you can use the ROS pluginlib
  // interface to load any planner that you want to use. Before we 
  // can load the planner, we need two objects, a RobotModel 
  // and a PlanningScene.
  // We will start by instantiating a
  // `RobotModelLoader`_
  // object, which will look up
  // the robot description on the ROS parameter server and construct a
  // :moveit_core:`RobotModel` for us to use.
  //
  // .. _RobotModelLoader: http://docs.ros.org/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

  
  // Using the :moveit_core:`RobotModel`, we can construct a
  // :planning_scene:`PlanningScene` that maintains the state of 
  // the world (including the robot). 
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

  // We will now construct a loader to load a planner, by name. 
  // Note that we are using the ROS pluginlib library here.
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  // We will get the name of planning plugin we want to load
  // from the ROS param server, and then load the planner
  // making sure to catch all exceptions.
  if (!node_handle.getParam("planning_plugin", planner_plugin_name))
    ROS_FATAL_STREAM("Could not find planner plugin name");
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
      ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
  }
  catch(pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (std::size_t i = 0 ; i < classes.size() ; ++i)
      ss << classes[i] << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                     << "Available plugins: " << ss.str());
  }

  /* Sleep a little to allow time to startup rviz, etc. */
  // ros::WallDuration sleep_time(15.0);
  ros::WallDuration sleep_time(1);
  sleep_time.sleep();

  // Pose Goal
  // ^^^^^^^^^
  // We will now create a motion plan request for the right arm of the PR2
  // specifying the desired pose of the end-effector as input.
  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;
  geometry_msgs::PoseStamped pose;
  pose.header.frame_id = "base";
  pose.pose.position.x = 0.3;
  pose.pose.position.y = 0.0;
  pose.pose.position.z = 0.3;
  pose.pose.orientation.w = 1.0;

  // A tolerance of 0.01 m is specified in position
  // and 0.01 radians in orientation
  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);

  // We will create the request as a constraint using a helper function available 
  // from the 
  // `kinematic_constraints`_
  // package.
  //
  // .. _kinematic_constraints: http://docs.ros.org/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
  req.group_name = "manipulator";
  moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
  req.goal_constraints.push_back(pose_goal);

  // We now construct a planning context that encapsulate the scene,
  // the request and the response. We call the planner using this 
  // planning context
  planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  if(res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully: %d", (int) res.error_code_.val);
    return 0;
  }

  // Visualize the result
  // ^^^^^^^^^^^^^^^^^^^^
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  moveit_msgs::MotionPlanResponse response;
  res.getMessage(response);

  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  display_publisher.publish(display_trajectory);

  sleep_time.sleep();

  // Joint Space Goals
  // ^^^^^^^^^^^^^^^^^
  /* First, set the state in the planning scene to the final state of the last plan */
  robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
  planning_scene->setCurrentState(response.trajectory_start);


#if 0


  const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("manipulator");
  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  // Now, setup a joint space goal
  robot_state::RobotState goal_state(robot_model);
  std::vector<double> joint_values(7, 0.0);
  joint_values[0] = -2.0;
  joint_values[3] = -0.2;
  joint_values[5] = -0.15;
  goal_state.setJointGroupPositions(joint_model_group, joint_values);
  moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
  req.goal_constraints.clear();
  req.goal_constraints.push_back(joint_goal);

  // Call the planner and visualize the trajectory
  /* Re-construct the planning context */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  /* Call the Planner */
  context->solve(res);
  /* Check that the planning was successful */
  if(res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }
  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  res.getMessage(response);
  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);

  /* Now you should see two planned trajectories in series*/
  display_publisher.publish(display_trajectory);

  /* We will add more goals. But first, set the state in the planning 
     scene to the final state of the last plan */
  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);

  /* Now, we go back to the first goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal);
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  display_publisher.publish(display_trajectory);

  // Adding Path Constraints
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
  /* Let's create a new pose goal */
  pose.pose.position.x = 0.65;
  pose.pose.position.y = -0.2;
  pose.pose.position.z = -0.1;
  moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("tool0", pose, tolerance_pose, tolerance_angle);
  /* First, set the state in the planning scene to the final state of the last plan */
  robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  /* Now, let's try to move to this new pose goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal_2);

  /* But, let's impose a path constraint on the motion.
     Here, we are asking for the end-effector to stay level*/
  geometry_msgs::QuaternionStamped quaternion;
  quaternion.header.frame_id = "torso_lift_link";
  quaternion.quaternion.w = 1.0;
  req.path_constraints = kinematic_constraints::constructGoalConstraints("tool0", quaternion);

  // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
  // (the workspace of the robot)
  // because of this, we need to specify a bound for the allowed planning volume as well;
  // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
  // but that is not being used in this example).
  // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
  // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z =  2.0;

  // Call the planner and visualize all the plans created so far.
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  // Now you should see four planned trajectories in series
  display_publisher.publish(display_trajectory);


#endif


  //END_TUTORIAL
  sleep_time.sleep();
  ROS_INFO("Done");
  planner_instance.reset();

  return 0;
}
Ejemplo n.º 29
0
int main(int argc, char **argv)
{
ros::init (argc, argv, "planning_scene_ros_api_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
ros::Duration sleep_time(5.0);
sleep_time.sleep();
sleep_time.sleep();
// BEGIN_TUTORIAL
//
// ROS API
// ^^^^^^^
// The ROS API to the planning scene publisher is through a topic interface
// using "diffs". A planning scene diff is the difference between the current
// planning scene (maintained by the move_group node) and the new planning
// scene desired by the user.
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "base_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "base_link";
/* The id of the object */
attached_object.object.id = "box";
/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x=0.0;
pose.position.y=-0.45;
pose.position.z=0.2;


/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.04;
primitive.dimensions[1] = 0.04;
primitive.dimensions[2] = 0.04;
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);





// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// Attach an object to the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// When the robot picks up an object from the environment, we need to
// "attach" the object to the robot so that any component dealing with
// the robot model knows to account for the attached object, e.g. for
// collision checking.
// Attaching an object requires two operations
// * Removing the original object from the environment
// * Attaching the object to the robot
/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// Detach an object from the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Detaching an object from the robot requires two operations
// * Detaching the object from the robot
// * Re-introducing the object into the environment
/* First, define the DETACH object message*/
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "gripper_roll_link";
detach_object.object.operation = attached_object.object.REMOVE;
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the DETACH + ADD operation */
ROS_INFO("Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// REMOVE THE OBJECT FROM THE COLLISION WORLD
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Removing the object from the collision world just requires
// using the remove object message defined earlier.
// Note, also how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
ROS_INFO("Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher.publish(planning_scene);
// END_TUTORIAL
ros::shutdown();
return 0;
}
Ejemplo n.º 30
0
int main(int argc, char **argv)
{
  ros::init (argc, argv, "motion_planning");
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::NodeHandle node_handle("/move_group");
//  ros::NodeHandle node_handle("~");

  /* SETUP A PLANNING SCENE*/
  /* Load the robot model */
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");

  /* Get a shared pointer to the model */
  robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

  /* Construct a planning scene - NOTE: this is for illustration purposes only.
     The recommended way to construct a planning scene is to use the planning_scene_monitor
     to construct it for you.*/
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

  /* SETUP THE PLANNER*/
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager> > planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  /* Get the name of the planner we want to use */
  if (!node_handle.getParam("planning_plugin", planner_plugin_name))
    ROS_FATAL_STREAM("Could not find planner plugin name");

  /* Make sure to catch all exceptions */
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>("moveit_core", "planning_interface::PlannerManager"));
  }
  catch(pluginlib::PluginlibException& ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
      ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
  }
  catch(pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string> &classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (std::size_t i = 0 ; i < classes.size() ; ++i)
      ss << classes[i] << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                     << "Available plugins: " << ss.str());
  }

  /* Sleep a little to allow time to startup rviz, etc. */
  ros::WallDuration sleep_time(1.0);
  sleep_time.sleep();

  /* CREATE A MOTION PLAN REQUEST FOR THE RIGHT ARM OF THE PR2 */
  /* We will ask the end-effector of the PR2 to go to a desired location */
  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;

  /* A desired pose */
  geometry_msgs::PoseStamped pose;
  pose.header.frame_id = "base_link";
  pose.pose.position.x = 0.3;
  pose.pose.position.y = -0.3;
  pose.pose.position.z = 0.7;

  pose.pose.orientation.x = 0.62478;
  pose.pose.orientation.y = 0.210184;
  pose.pose.orientation.z = -0.7107 ;
  pose.pose.orientation.w = 0.245722;

  /* A desired tolerance */
  std::vector<double> tolerance_pose(3, 0.1);
  std::vector<double> tolerance_angle(3, 0.1);
//  std::vector<double> tolerance_pose(3, 0.01);
 // std::vector<double> tolerance_angle(3, 0.01);

  ROS_INFO("marker4");
  /*Create the request */
  req.group_name = "manipulator";
  moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);
  req.goal_constraints.push_back(pose_goal);
  ROS_INFO("marker5");

  /* Construct the planning context */
  planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  ROS_INFO("marker6");

  /* CALL THE PLANNER */
//  context->solve(res);
//  ROS_INFO("marker7");

  /* Check that the planning was successful */
  if(res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }


  /* Visualize the generated plan */
  /* Publisher for display */
  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  moveit_msgs::MotionPlanResponse response;
  res.getMessage(response);

  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  display_publisher.publish(display_trajectory);

  sleep_time.sleep();

  /* NOW TRY A JOINT SPACE GOAL */
  /* First, set the state in the planning scene to the final state of the last plan */
  robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
  planning_scene->setCurrentState(response.trajectory_start);
  robot_state::JointStateGroup* joint_state_group = robot_state.getJointStateGroup("manipulator");
  joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);

  /* Now, setup a joint space goal*/
  robot_state::RobotState goal_state(robot_model);
  robot_state::JointStateGroup* goal_group = goal_state.getJointStateGroup("manipulator");
  std::vector<double> joint_values(7, 0.0);
//  joint_values[0] = 2.0;
  joint_values[2] = 1.6;
//  joint_values[5] = -0.15;
  goal_group->setVariableValues(joint_values);
  moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_group);

  req.goal_constraints.clear();
  req.goal_constraints.push_back(joint_goal);

  /* Construct the planning context */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);

  /* Call the Planner */
  context->solve(res);

  /* Check that the planning was successful */
  if(res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }

  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  res.getMessage(response);

  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  //Now you should see two planned trajectories in series
  display_publisher.publish(display_trajectory);

  /* Now, let's try to go back to the first goal*/
  /* First, set the state in the planning scene to the final state of the last plan */
  joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);

  /* Now, we go back to the first goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal);
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  display_publisher.publish(display_trajectory);

  /* Let's create a new pose goal */
  pose.pose.position.x = 0.65;
  pose.pose.position.y = -0.2;
  pose.pose.position.z = -0.1;
  moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("wrist_3_link", pose, tolerance_pose, tolerance_angle);

  /* First, set the state in the planning scene to the final state of the last plan */
  joint_state_group->setVariableValues(response.trajectory.joint_trajectory.points.back().positions);

  /* Now, let's try to move to this new pose goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal_2);

  /* But, let's impose a path constraint on the motion.
     Here, we are asking for the end-effector to stay level*/
  geometry_msgs::QuaternionStamped quaternion;
  quaternion.header.frame_id = "base_link";
  quaternion.quaternion.w = 1.0;

  req.path_constraints = kinematic_constraints::constructGoalConstraints("wrist_3_link", quaternion);

  // imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
  // (the workspace of the robot)
  // because of this, we need to specify a bound for the allowed planning volume as well;
  // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
  // but that is not being used in this example).
  // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done in this volume
  // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =  -2.0;
  req.workspace_parameters.min_corner.z = 0.2;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z =  2.0;


  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  //Now you should see four planned trajectories in series
  display_publisher.publish(display_trajectory);

  sleep_time.sleep();
  ROS_INFO("Done");
  planner_instance.reset();

  return 0;
}