// ========================================================================= // 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); } } } }
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; }
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; }
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 }
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; } } }
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; }
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 }
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 } } }
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(); }
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; }
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)); } }
/** * 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"); }
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; }
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(); }
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; }
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(); } }
//------------------------------------------------------------------------------ /// 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 --------- }
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; }
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; }
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; }
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; }
/** * @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; }
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; }
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; }
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; }