Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

merged from frcsim branch

Browse Source 
verified to work on real robots
adds sim eclipse plugins, fixed JavaGazebo, made wpilibC++Sim build on windows
 - Java and C++ simulation robot programs run on windows
 - simulation eclipse plugin delivers models and gazebo plugins
 - Java Gazebo now respects GAZEBO_IP variables and can work across networks
 - hal and network tables win32 hacked to work on windows
 - smart dashboard broken on windows due to network tables hacks
 - wpilibC++Sim, gz_msgs, and frcsim_gazebo_plugins build with CMake
 - removed constexpr for cross platform compatibility
 - msgs generated using .protos as a part of build process
 - some spare and unused cmake/pom files deleted
 - simulation ubuntu debians removed entirely
 - refactored CMake project flags and macros
 - updated to match non-sim C++ API
 - fixed and updated documentation
 - servo added to simulation

Change-Id: Ia702ff0f1fee10d77f543810ad88f56696443b05
This commit is contained in:
peter mitrano
2015-04-26 19:19:57 -04:00
parent 4e46692191
commit 29d029fa61
211 changed files with 2143 additions and 6491 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
networktables/cpp/lib/Win32/src/main/native/OSAL/Synchronized.cpp
View File

@@ -3,78 +3,22 @@
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "stdafx.h"
// Previous version used semaphores.
// This version was copied from the "Athena" version of network tables.
// It uses pthread and is much simpler
#include "OSAL/Synchronized.h"
#include <stdlib.h>
//TODO see what the STATUS is suppose to return for success
STATUS semGive (SEM_ID semId)
pthread_mutexattr_t mta;
bool hasInit = false;
NTSynchronized::NTSynchronized(NTReentrantSemaphore& semaphore):m_semaphore(semaphore)
{
::LeaveCriticalSection( semId );
return 0;
}
STATUS semTake (SEM_ID semId, int timeout)
{
if (timeout==WAIT_FOREVER)
::EnterCriticalSection( semId );
else
{
BOOL result;
int TimeOut=0;
do
{
result=::TryEnterCriticalSection( semId );
if (result==0)
Sleep(10);
} while ((result==0)&&(TimeOut++<timeout));
assert(result!=0); //TODO timeout
}
return 0;
m_semaphore.take();
}
SEM_ID semMCreate (int options)
{
SEM_ID ret=new CRITICAL_SECTION;
::InitializeCriticalSection( ret );
return ret;
}
STATUS semDelete (SEM_ID semId)
{
::DeleteCriticalSection( semId );
delete semId;
return 0;
}
/**
* Synchronized class deals with critical regions.
* Declare a Synchronized object at the beginning of a block. That will take the semaphore.
* When the code exits from the block it will call the destructor which will give the semaphore.
* This ensures that no matter how the block is exited, the semaphore will always be released.
* Use the CRITICAL_REGION(SEM_ID) and END_REGION macros to make the code look cleaner (see header file)
* @param semaphore The semaphore controlling this critical region.
*/
NTSynchronized::NTSynchronized(SEM_ID semaphore)
{
usingSem = false;
m_semaphore = semaphore;
semTake(m_semaphore, WAIT_FOREVER);
}
NTSynchronized::NTSynchronized(NTReentrantSemaphore& semaphore)
{
usingSem = true;
m_sem = &semaphore;
m_sem->take();
}
/**
* This destructor unlocks the semaphore.
*/
NTSynchronized::~NTSynchronized()
{
if(usingSem)
m_sem->give();
else
semGive(m_semaphore);
m_semaphore.give();
}

6
networktables/cpp/lib/Win32/src/main/native/OSAL/System.cpp
View File

@@ -6,15 +6,17 @@
* Author: Mitchell Wills
*/
#include "windows.h"
#include <Windows.h>
#include <Mmsystem.h>
#include "networktables2/util/System.h"
//#include "semLib.h"
#include <stdio.h>
//#include <sysLib.h> // for sysClkRateGet
//#include <usrLib.h> // for taskDelay
//timeGetTime() uses Winmm.lib
//timeGetTime() uses Winmm.lib
#pragma comment (lib,"Winmm.lib")
#pragma comment( lib, "Ws2_32" )
void sleep_ms(unsigned long ms){
//taskDelay((INT32)((double)sysClkRateGet() * ms / 1000));
Sleep(ms);

316
networktables/cpp/lib/Win32/src/main/native/OSAL/Task.cpp
View File

@@ -1,316 +0,0 @@
#include "stdafx.h"
#include "OSAL/Task.h"
#include "NetworkCommunication/UsageReporting.h"
#include "WPIErrors.h"
#include <string.h>
#include <Windows.h>
//const UINT32 NTTask::kDefaultPriority;
//const INT32 NTTask::kInvalidTaskID;
/**
* Create but don't launch a task.
* @param name The name of the task. "FRC_" will be prepended to the task name.
* @param function The address of the function to run as the new task.
* @param priority The priority for the task.
* @param stackSize The size of the stack for the task
*/
NTTask::NTTask(const char* name, FUNCPTR function, INT32 priority, UINT32 stackSize)
{
//m_taskID = kInvalidTaskID;
m_Handle=NULL;
m_function = function;
m_priority = priority;
m_stackSize = stackSize;
m_taskName = new char[strlen(name) + 5];
strcpy(m_taskName, "FRC_");
strcpy(m_taskName+4, name);
//TODO see if we want to debug out this... it may be interesting info
#if 0
static INT32 instances = 0;
instances++;
#endif
}
NTTask::~NTTask()
{
//if (m_taskID != kInvalidTaskID) Stop();
if (m_Handle)
Stop();
delete [] m_taskName;
m_taskName = NULL;
}
// The thread callback
DWORD thread_proc( void *p_ptr )
{ // Get the pointer to myself
NTTask *p_this = (NTTask*)p_ptr;
assert( p_this );
(*p_this->m_function)( p_this->m_Arg );
return 0;
}
//This sets the name of the thread, which can help to identify threads in win32
#define MS_VC_EXCEPTION 0x406D1388
static void set_thread_name( const char *p_thread_name, DWORD ID )
{
#pragma pack(push,8)
typedef struct tagTHREADNAME_INFO
{ DWORD dwType; // Must be 0x1000.
LPCSTR szName; // Pointer to name (in user addr space).
DWORD dwThreadID; // Thread ID (-1=caller thread).
DWORD dwFlags; // Reserved for future use, must be zero.
} THREADNAME_INFO;
#pragma pack(pop)
// Set the information
THREADNAME_INFO info = { 0x1000, p_thread_name, ID, 0 };
// Raise the exception
__try { ::RaiseException( MS_VC_EXCEPTION, 0, sizeof(info)/sizeof(ULONG_PTR), (ULONG_PTR*)&info ); }
__except( EXCEPTION_EXECUTE_HANDLER ) {}
}
bool NTTask::StartInternal()
{
if (m_Handle)
{
assert(false); // This may be lifted... just want to see if it happens
Stop();
}
m_Handle = ::CreateThread( NULL, m_stackSize, (LPTHREAD_START_ROUTINE)thread_proc, (void*)this, NULL, &m_ID );
if (m_ID!=NULL)
set_thread_name(m_taskName,m_ID);
return m_Handle!=NULL;
}
/**
* Starts this task.
* If it is already running or unable to start, it fails and returns false.
*/
bool NTTask::Start(void *arg0)
{
m_Arg=arg0;
return StartInternal();
}
/**
* Restarts a running task.
* If the task isn't started, it starts it.
* @return false if the task is running and we are unable to kill the previous instance
*/
bool NTTask::Restart()
{
//return HandleError(taskRestart(m_taskID));
Stop();
return StartInternal();
}
/**
* Kills the running task.
* @returns true on success false if the task doesn't exist or we are unable to kill it.
*/
bool NTTask::Stop()
{
if (!m_Handle) return false;
bool ok = true;
// Wait for the thread to finish
#ifdef _DEBUG
try_again:
#endif _DEBUG
//const int TimeOut=2000;
const int TimeOut=INFINITE;
if ( ::WaitForSingleObject( m_Handle , TimeOut ) == WAIT_TIMEOUT )
{ // Signal the thread as having been terminated
//if ( m_p_error ) *m_p_error = true;
// If this gets triggered we have a bug in the code.
#ifdef _DEBUG
switch( ::MessageBoxW( NULL, L"A thread being used by the application\n"
L"has taken to long to exit and so is about\n"
L"to be terminated to avoid locking-up\n"
L"the application.\n\n"
L"Click ABORT to debug.\n"
L"Click RETRY to wait for a bit longer.\n"
L"Click IGNORE to terminate the thread.\n\n"
L"This message is NOT displayed in release mode.",
L"Thread exit has timed out.",
MB_ABORTRETRYIGNORE ) )
{ case IDRETRY: goto try_again;
case IDABORT: ::DebugBreak(); break;
case IDIGNORE: break;
}
#endif _DEBUG
// Free thread memory
CONTEXT c_ = {0};
c_.ContextFlags = CONTEXT_FULL;
::GetThreadContext( m_Handle, &c_ );
MEMORY_BASIC_INFORMATION Info_ = {0};
#ifdef _M_X64
::VirtualQuery( (PVOID) c_.Rsp, &Info_, sizeof(Info_) );
#else
::VirtualQuery( (PVOID) c_.Esp, &Info_, sizeof(Info_) );
#endif
// Terminate the thread
::TerminateThread( m_Handle, 0 );
// Free the memory
::VirtualFree( Info_.AllocationBase, 0, MEM_RELEASE );
}
//if (Verify())
//{
// ok = HandleError(taskDelete(m_taskID));
//}
//m_taskID = kInvalidTaskID;
// The thread has finished
CloseHandle( m_Handle );
m_Handle = NULL;
return ok;
}
/**
* Returns true if the task is ready to execute (i.e. not suspended, delayed, or blocked).
* @return true if ready, false if not ready.
*/
bool NTTask::IsReady()
{
//return taskIsReady(m_taskID);
return m_Handle!=NULL;
}
/**
* Returns true if the task was explicitly suspended by calling Suspend()
* @return true if suspended, false if not suspended.
*/
bool NTTask::IsSuspended()
{
//return taskIsSuspended(m_taskID);
return false;
}
/**
* Pauses a running task.
* Returns true on success, false if unable to pause or the task isn't running.
*/
bool NTTask::Suspend()
{
//return HandleError(taskSuspend(m_taskID));
assert(false);
return false;
}
/**
* Resumes a paused task.
* Returns true on success, false if unable to resume or if the task isn't running/paused.
*/
bool NTTask::Resume()
{
//return HandleError(taskResume(m_taskID));
assert(false);
return false;
}
/**
* Verifies a task still exists.
* @returns true on success.
*/
bool NTTask::Verify()
{
//return taskIdVerify(m_taskID) == OK;
return true;
}
/**
* Gets the priority of a task.
* @returns task priority or 0 if an error occured
*/
INT32 NTTask::GetPriority()
{
//if (HandleError(taskPriorityGet(m_taskID, &m_priority)))
// return m_priority;
//else
// return 0;
return m_priority;
}
/**
* This routine changes a task's priority to a specified priority.
* Priorities range from 0, the highest priority, to 255, the lowest priority.
* Default task priority is 100.
* @param priority The priority the task should run at.
* @returns true on success.
*/
bool NTTask::SetPriority(INT32 priority)
{
m_priority = priority;
//return HandleError(taskPrioritySet(m_taskID, m_priority));
return true;
}
/**
* Returns the name of the task.
* @returns Pointer to the name of the task or NULL if not allocated
*/
const char* NTTask::GetName()
{
return m_taskName;
}
/**
* Get the ID of a task
* @returns Task ID of this task. NTTask::kInvalidTaskID (-1) if the task has not been started or has already exited.
*/
INT32 NTTask::GetID()
{
//if (Verify())
// return m_taskID;
if (m_Handle)
return m_ID;
return kInvalidTaskID;
}
/**
* Handles errors generated by task related code.
*/
bool NTTask::HandleError(STATUS results)
{
if (results != ERROR) return true;
//switch(errnoGet())
//{
//case S_objLib_OBJ_ID_ERROR:
// wpi_setWPIErrorWithContext(TaskIDError, m_taskName);
// break;
//
//case S_objLib_OBJ_DELETED:
// wpi_setWPIErrorWithContext(TaskDeletedError, m_taskName);
// break;
//
//case S_taskLib_ILLEGAL_OPTIONS:
// wpi_setWPIErrorWithContext(TaskOptionsError, m_taskName);
// break;
//
//case S_memLib_NOT_ENOUGH_MEMORY:
// wpi_setWPIErrorWithContext(TaskMemoryError, m_taskName);
// break;
//
//case S_taskLib_ILLEGAL_PRIORITY:
// wpi_setWPIErrorWithContext(TaskPriorityError, m_taskName);
// break;
//default:
// printErrno(errnoGet());
// wpi_setWPIErrorWithContext(TaskError, m_taskName);
//}
return false;
}

22
networktables/cpp/lib/Win32/src/main/native/networktables2/stream/FDIOStream.cpp
View File

@@ -1,22 +1,24 @@
#include "stdafx.h"
/*
* FDIOStream.cpp
*
* Created on: Sep 27, 2012
*/
//make sure this comes before windows.h
#include <winsock2.h>
#include "networktables2/stream/FDIOStream.h"
#include "networktables2/util/IOException.h"
#include "networktables2/util/EOFException.h"
#include <iostream>
#include <errno.h>
#include <stdlib.h>
#include <assert.h>
#include <stdio.h>
#include <windows.h>
#include <winsock2.h>
#include <wininet.h>
#include <ws2tcpip.h>
#include <io.h>
FDIOStream::FDIOStream(int _fd){
fd = _fd;
@@ -24,7 +26,7 @@ FDIOStream::FDIOStream(int _fd){
u_long on = 1;
if (ioctlsocket(fd, FIONBIO, &on))
{
::close(fd);
::_close(fd);
throw IOException("Could not set socket to non-blocking mode");
}
}
@@ -38,7 +40,7 @@ int FDIOStream::read(void* ptr, int numbytes){
return 0;
char* bufferPointer = (char*)ptr;
int totalRead = 0;
while (totalRead < numbytes)
while (totalRead < numbytes)
{
int numRead=recv(fd, bufferPointer, numbytes-totalRead, 0);
if(numRead == 0){
@@ -77,7 +79,7 @@ int Send( int sockfd,char* Data, size_t sizeData )
if (!Result_)
{
char Buffer[128];
sprintf(Buffer,"Send() failed: WSA error=%d\n",WSAGetLastError());
sprintf_s(Buffer,"Send() failed: WSA error=%d\n",WSAGetLastError());
OutputDebugStringA(Buffer);
}
@@ -86,7 +88,7 @@ int Send( int sockfd,char* Data, size_t sizeData )
int FDIOStream::write(const void* ptr, int numbytes)
{
int numWrote = ::write(fd, (char*)ptr, numbytes);
int numWrote = ::_write(fd, (char*)ptr, numbytes);
if(numWrote==numbytes)
return numWrote;
@@ -106,12 +108,12 @@ int FDIOStream::write(const void* ptr, int numbytes)
throw IOException("Select returned an error on write");
if (FD_ISSET(fd, &fdSet)) {
numWrote = ::write(fd, (char*)ptr, numbytes);
numWrote = ::_write(fd, (char*)ptr, numbytes);
if(numWrote==numbytes)
return numWrote;
}
}
perror("write error: ");
fflush(stderr);
throw IOException("Could not write all bytes to fd stream");
@@ -126,7 +128,7 @@ void FDIOStream::close()
if (fd != INVALID_SOCKET)
{
char Buffer[128];
sprintf(Buffer,"closesocket %d\n",fd);
sprintf_s(Buffer,"closesocket %d\n",fd);
OutputDebugStringA(Buffer);
shutdown( fd, SD_BOTH );

2
networktables/cpp/lib/Win32/src/main/native/networktables2/stream/SocketServerStreamProvider.cpp
View File

@@ -4,7 +4,6 @@
* Created on: Sep 27, 2012
* Author: Mitchell Wills
*/
#include "stdafx.h"
#include "networktables2/stream/SocketServerStreamProvider.h"
#include "networktables2/stream/FDIOStream.h"
@@ -12,6 +11,7 @@
#include <cstring>
#include <errno.h>
#include <assert.h>
#include <stdlib.h>
#include <stdio.h>
#include <sys/types.h>

9
networktables/cpp/lib/Win32/src/main/native/networktables2/stream/SocketStreamFactory.cpp
View File

@@ -4,10 +4,9 @@
* Created on: Nov 3, 2012
* Author: Mitchell Wills
*/
#include "stdafx.h"
#include <cstring>
#include <stdlib.h>
#include <assert.h>
#include <stdio.h>
#include <sys/types.h>
#include <winsock2.h>
@@ -53,7 +52,7 @@ IOStream *SocketStreamFactory::createStream(){
server = gethostbyname(host);
if (server == NULL)
if (server == NULL)
throw 1;
memset(&serv_addr, 0, sizeof(serv_addr));
@@ -75,7 +74,7 @@ IOStream *SocketStreamFactory::createStream(){
// [9/10/2013 JamesK]
{
char Buffer[128];
sprintf(Buffer,"connecting %d\n",sockfd);
sprintf_s(Buffer,"connecting %d\n",sockfd);
OutputDebugStringA(Buffer);
}
if (connect(sockfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) == 0)
@@ -117,7 +116,7 @@ IOStream *SocketStreamFactory::createStream(){
if (ErrorMsg)
{
char Buffer[1024];
sprintf(Buffer,"ErrorMsg=%s WSA error=%d\n",ErrorMsg,WSAGetLastError());
sprintf_s(Buffer,"ErrorMsg=%s WSA error=%d\n",ErrorMsg,WSAGetLastError());
OutputDebugStringA(Buffer);
printf("ErrorMsg=%s WSA error=%d\n",ErrorMsg,WSAGetLastError());

68
networktables/cpp/lib/Win32/src/main/native/networktables2/thread/DefaultThreadManager.cpp
View File

@@ -3,58 +3,44 @@
*
* Created on: Sep 21, 2012
* Author: Mitchell Wills
* Re-Created on: June 26, 2015
* Author: Peter Mitrano
*/
#include "../../../../../../../../stdafx.h"
// Previous version used semaphores.
// This version was copied from the "Athena" version of network tables.
// It uses pthread and is much simpler
#include "networktables2/thread/DefaultThreadManager.h"
#include <exception>
#include <stdio.h>
PeriodicNTThread::PeriodicNTThread(PeriodicRunnable* _r, const char* _name) :
name(_name), thread(new NTTask(name, (FUNCPTR)PeriodicNTThread::taskMain)), r(_r), run(true)
{
fprintf(stdout, "Starting task: %s\n", name);
fflush(stdout);
thread->Start(this);
PeriodicNTThread::PeriodicNTThread(PeriodicRunnable* _r, const char* name) : r(_r), run(true){
if(pthread_create(&thread, NULL, (void* (*)(void*))PeriodicNTThread::taskMain, (void*)this))
throw std::exception();
}
PeriodicNTThread::~PeriodicNTThread()
{
stop();
//TODO somehow do this async
if (thread)
{
delete thread;
thread=NULL;
}
PeriodicNTThread::~PeriodicNTThread(){
stop();
if(thread.x == pthread_self().x){
fprintf(stderr, "WARNING: thread destructor called from this thread\n");
}
pthread_join(thread, NULL);
}
int PeriodicNTThread::taskMain(PeriodicNTThread* o)
{
//static wrapper
return o->_taskMain();
}
int PeriodicNTThread::_taskMain(){
try {
while(run){
r->run();
}
} catch (...) {
fprintf(stdout, "Task exited with uncaught exception %s\n", name);
fflush(stdout);
return 1;
}
fprintf(stdout, "Task exited normally: %s\n", name);
fflush(stdout);
void* PeriodicNTThread::taskMain(PeriodicNTThread* o){//static wrapper
o->_taskMain();
return 0;
}
void PeriodicNTThread::stop()
{
run = false;
void PeriodicNTThread::_taskMain(){
while(run){
r->run();
}
}
void PeriodicNTThread::stop() {
run = false;
//pthread_cancel(thread);
}
bool PeriodicNTThread::isRunning() {
return thread->IsReady();
return pthread_kill(thread, 0) == 0;
}
NTThread* DefaultThreadManager::newBlockingPeriodicThread(PeriodicRunnable* r, const char* name) {