int main(int argc, char **argv)
{
// just some stuff for returns
std::string str;
int ret;
// robots connection
ArSerialConnection con;
// the robot, this turns state reflection off
ArRobot robot(NULL, false);
// the joydrive as defined above, this also adds itself as a user task
Joydrive joyd(&robot);
// mandatory init
Aria::init();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the connection o nthe robot
robot.setDeviceConnection(&con);
// connect, if we fail, exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn off the sonar, enable the motors, turn off amigobot sounds
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// run, if we lose connection to the robot, exit
robot.run(true);
// shutdown and go away
Aria::shutdown();
return 0;
}
int main(int argc, char **argv) {
ros::init(argc, argv, "robot_state", ros::init_options::AnonymousName);
ros::NodeHandle node("~");
ros::AsyncSpinner spinner(1);
spinner.start();
clopema_robot::ClopemaRobotCommander robot("arms");
geometry_msgs::Pose p;
p = robot.getCurrentPose("r1_ee").pose;
showPose(p);
p = robot.getCurrentPose("r2_ee").pose;
showPose(p);
ros::shutdown();
return 0;
}
bool WorkspaceLatticeBase::stateWorkspaceToRobot(
const WorkspaceState& state,
RobotState& ostate) const
{
RobotState seed(robot()->jointVariableCount(), 0);
for (size_t fai = 0; fai < freeAngleCount(); ++fai) {
seed[m_fangle_indices[fai]] = state[6 + fai];
}
Affine3 pose =
Translation3(state[0], state[1], state[2]) *
AngleAxis(state[5], Vector3::UnitZ()) *
AngleAxis(state[4], Vector3::UnitY()) *
AngleAxis(state[3], Vector3::UnitX());
return m_rm_iface->computeFastIK(pose, seed, ostate);
}
int main(int argc, char **argv)
{
std::string str;
int ret;
// connection
ArTcpConnection con;
// robot, this starts it with state reflecting turned off
ArRobot robot(NULL, false);
// make the joydrive instance, which adds its task to the robot
Joydrive joyd(&robot);
// mandatory aria initialization
Aria::init();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::exit(1);
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
return 1;
}
// turn off sonar, turn the motors on, turn off amigobot sound
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
// run the robot, true so that if connection is lost the run stops
robot.run(true);
// now exit
Aria::exit(0); // exit program
return 0;
}
void UserProgram::toggleState()
{
QFileInfo robot("/mnt/user/bin/robot");
if(m_userProgram.state() == QProcess::NotRunning) {
if(robot.exists()) {
m_userProgram.start("/mnt/kiss/usercode/run");
}
else {
emit consoleOutput(QString("No program to run!\n"));
}
}
else {
emit consoleOutput(QString("Program Stopped!\n"));
m_userProgram.terminate();
emit stopped();
}
}
int main(int argc, char **argv)
{
const char* logFileName = "server.log";
int port = 9999;
int opt;
opterr = 0;
while ((opt = getopt (argc, argv, "l:p:")) != -1) {
switch(opt) {
case 'l':
logFileName = optarg;
break;
case 'p':
port = atoi(optarg);
break;
case '?':
printUsage(argv[0]);
return 1;
default:
printUsage(argv[0]);
return 1;
}
}
// define and configure device
UARTDevice::UARTDeviceConfig cfg;
cfg.baudRate = UARTDevice::UARTDeviceConfig::BAUD_38400;
cfg.deviceName = "/dev/ttyS0";
UARTDevice serialDevice;
serialDevice.open(cfg);
std::ofstream logFile(logFileName);
Logger::initialize(LDEBUG, &logFile);
/// Disabling the daemonisation: since we now launch the process
/// through inittab using respawn, it can not be a daemon.
//daemon(0,0);
RobotService robot(serialDevice, port);
robot.run();
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "epos_hw_node");
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
double controller_rate;
pnh.param<double>("controller_rate", controller_rate, 10);
std::vector<std::string> epos_names;
pnh.getParam("names", epos_names);
epos_hardware::EposHardware robot(nh, pnh, epos_names);
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO("Initializing Motors");
if(!robot.init()) {
ROS_FATAL("Failed to initialize motors");
return 1;
}
ROS_INFO("Motors Initialized");
controller_manager::ControllerManager cm(&robot, pnh);
ros::Timer controller_load_timer = walrus_base_hw::createControllerLoadTimer(pnh, &cm);
ROS_INFO("Running loop");
walrus_base_hw::RealtimeRate rate(controller_rate);
while (ros::ok()) {
ros::Duration dt;
ros::Time now;
rate.beginLoop(&now, &dt);
robot.read();
cm.update(now, dt);
robot.write();
robot.update_diagnostics();
rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
// just some stuff for returns
std::string str;
// robots connection
ArSerialConnection con;
// the robot, this turns state reflection off
ArRobot robot(NULL, false);
// the joydrive as defined above, this also adds itself as a user task
KeyPTU ptu(&robot);
// mandatory init
Aria::init();
ArLog::init(ArLog::StdOut, ArLog::Terse, NULL, true);
con.setPort(ArUtil::COM1);
// set the connection on the robot
robot.setDeviceConnection(&con);
// connect, if we fail, exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// turn off the sonar, enable the motors, turn off amigobot sounds
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
printf("Press '?' for available commands\r\n");
// run, if we lose connection to the robot, exit
robot.run(true);
// shutdown and go away
Aria::shutdown();
return 0;
}
int main(int argc, char **argv)
{
Aria::init();
// command-line arguments and robots connection
ArArgumentParser argParser(&argc, argv);
argParser.loadDefaultArguments();
ArSimpleConnector con(&argParser);
// the robot, but turn state reflection off (so we have no mobility control in
// this program)
ArRobot robot(NULL, false);
// an object for keyboard control, class defined above, this also adds itself as a user task
KeyPTU ptu(&robot);
// parse command-line arguments (i.e. connection options for simple connector)
if(!Aria::parseArgs())
{
Aria::logOptions();
return 1;
}
// connect to the robot
if (!con.connectRobot(&robot))
{
ArLog::log(ArLog::Terse, "Error connecting to robot!");
Aria::shutdown();
return 1;
}
// turn off the sonar
robot.comInt(ArCommands::SONAR, 0);
printf("Press '?' for available commands\r\n");
// run, if we lose connection to the robot, exit
robot.run(true);
Aria::shutdown();
return 0;
}
int main(int argc, char** argv)
{
mrcoreInit();
AriaBase robot(argc,argv);
WheeledBaseServer server(&robot,"base");
LaserSensorServer laserServer(&robot,"laser");
server.init(12300,1,true);
//laserServer.init(12001,1,true);
while(1)
{
Sleep(500);
robot.watchdog();
}
server.close();
LOG_INFO("End of server");
return 1;
}
int main(int argc, char *argv[])
{
// For handling Ctrl+C nicely
signal(SIGINT, signal_callback_handler);
// Create the Dimitri robot object connected
// to /dev/ttyUSB0
Dimitri robot(0, 4);
// Sets maximum torque to the joints
// P.S.: optionally can set for all motors
// like this: robot.setMaxTorque(64);
robot.getHead()->setMaxTorque(64);
robot.getWaist()->setMaxTorque(384);
robot.getRightArm()->setMaxTorque(256);
robot.getLeftArm()->setMaxTorque(256);
// Enable torque to the joints setting the
// teaching control mode
robot.setControlMode(TEACH);
// This array stores the pose angles
float pose[13];
// Main loop
while (true)
{
// Performs update
robot.update();
// Gets the pose angles
robot.getPose( pose );
// Print the pose angles
for (int i = 0 ; i < 13 ; i++)
{
printf("%+04.0f,", RAD2DEG(pose[i]));
}
printf("\n");
}
return 0;
}
void robot_solo(int t)
{
int T = t;
if ( t < 256 ) {
// stopped for the intro
t = 0;
} else if ( t > 1152 ) {
// tracted by the flying saucer
int y;
y = (t-1152)*58;
if ( y > 22528 )
y = 22528;
translate(0, y, 0);
y = (8*4096-y)/8;
scale(y, y, y);
t = 0;
}
robot(t, T);
}
int main()
{
// Read the configuration file
ConfigManager::ReadParameters();
// Read and inflate map
const char* mapurl = ConfigManager::GetMapUrl().erase(ConfigManager::GetMapUrl().size() - 1).c_str();
Map* currMap = LoadMap(mapurl);
int robotSize = MAX(ConfigManager::GetRobotHeight(),ConfigManager::GetRobotWidth());
double mapResulotion = ConfigManager::GetMapResolution();
int InflateAddition = ceil((robotSize / 2) / mapResulotion);
Map* inflateMap = currMap->Inflate(InflateAddition);
Map* Maparticle = currMap->Inflate(5);
// Calculate the ratio between the grid resolution and map resolution
int res = ConfigManager::GetGridResolution() / ConfigManager::GetMapResolution();
Grid* grid = new Grid(inflateMap->GetMatrix(),inflateMap->GetWidth(),inflateMap->GetHeight(), res);
// A*
PathPlanner* p = new PathPlanner();
Point* start = ConfigManager::GetStartLocationMapResolution();
Point* end = ConfigManager::GetGoalMapResolution();
int startGridPointX,startGridPointY, endGridPointX, endGridPointY;
startGridPointX = start->GetRow() / res;
startGridPointY = start->GetCol() / res;
endGridPointX = end->GetRow() / res;
endGridPointY = end->GetCol() / res;
std::string route = p->pathFind(startGridPointX ,startGridPointY,endGridPointX,endGridPointY,grid);
// WayPoint calculation
Point* startWithRes = new Point(start->GetRow() / 4, start->GetCol() / 4);
vector<Point *> waypointsArr = WayPoints::CalculateByDirectionalPath(route, startWithRes);
Robot robot("localhost",6665);
PlnObstacleAvoid plnOA(&robot);
Manager manager(&robot, &plnOA, waypointsArr, Maparticle);
manager.run();
}
void Coop::checkIddleRobots()
{
if(robots_.size() != 0){
for (int i = 0; i < robots_.size(); i++)
{
std::cout<<robots_.at(i).getName()<<": "<<robots_.at(i).getBusy()<<"\n";
}
}
int number_of_iddle_robots = 0;
for (int i = 0; i < robots_.size(); i++)
{
Robot robot(robots_.at(i));
//ROS_INFO("%s is logged!!!", robot.getName().c_str());
if(!robot.getBusy())
{
number_of_iddle_robots++;
}
}
//ROS_INFO("Number of robots: %d", number_of_iddle_robots);
}
int main(){
TCCR1A = (1<<COM1A1)|(1<<COM1B1)|(1<<WGM10); // COM1A1 pour comparaison sur OC1A
// WGM 13-10 = 0001 pour PWM 1 ( 12 et 13 sur B et 10 et 11 sur A)
TCCR1B = (1<<CS11); // CS 12-10 = 010 pour clk/8
TCNT1 = 1;
DDRD = 0x3F;
DDRB = 0xFF;
PORTB = 0;
_delay_ms (1000);
Parcours robot(1);
robot.effectuerEpreuve();
robot.effectuerEpreuve();
return 0;
}
int main(void) {
char input[6];
// printf("Hur ligger paketen? ");
// scanf("%s",input);
//Inläsning från fil med alla permutationer av ABCDE
FILE *fil;
fil=fopen("abcde.txt","rt");
char temp[6];
fscanf(fil,"%s", input);
while (input != NULL) {
strcpy(temp,input);
robot(input);
printf("Det kr\x84vs %2d steg f\x94r %s \n",antal,temp);
fscanf(fil,"%s", input);
antal = 0;
}
fclose(fil);
return 0;
}
// 在iPPid的前后3点寻找距离当前实际位置最近的点，输出为最新
int CPathAgent::UpdatePPindexNow(int iPPid)
{
mrpt::utils::CConfigFile PP_File(m_Path_ini_file);
int iStart, iEnd;
if (iPPid <= 3)
{
iStart = 0;
iEnd = iPPid+3;
}
else if(iPPid >= (m_iPathPointSum-3))
{
iStart = iPPid-3;
iEnd = m_iPathPointSum-1;
}
else
{
iStart = iPPid-3;
iEnd = iPPid+3;
}
CPose2D robot(g_pGyro->ReadPose());
std::vector<double> aux; // Auxiliar vector
std::stringstream ss;
double minDis = 1.0;
int minIndex = iPPid;
for (int i=iStart; i<= iEnd; i++)
{
ss.str("");
ss.clear();
ss<<"PP"<<i;
PP_File.read_vector("PathPoint", ss.str(), aux, aux);
double dis = robot.distance2DTo(aux.at(0), aux.at(1));
if (dis < minDis)
{
minDis = dis;
minIndex = i;
}
}
return minIndex;
}
void ShipBuildingArea::initialise()
{
ambientSound_ = new SoundEffect;
ambientSound_->initialise("audio/Background Ambience/Ambience1.wav");
ambientSound_->play(0);
escapePodBuildingPuzzle_.initialise();
sky_.initialise("scripts/ShipBuildingAreaSky.txt");
distantBackground_.initialise("scripts/ShipBuildingAreaDistantBackground.txt");
closeBackground_.initialise("scripts/ShipBuildingAreaCloseBackground.txt");
midground_.initialise("scripts/ShipBuildingAreaMidground.txt");
playerStage_.initialise("scripts/ShipBuildingAreaPlayerStage.txt");
foreground_.initialise("scripts/ShipBuildingAreaForeground.txt");
// Set alien x
alien()->setX(200);
robot()->setCenterX(150);
ending_ = new Video;
ending_->initialise("cutscenes/5Ending.mp4");
}
void ShipBuildingArea::deinitialise()
{
// Stop all sounds
ambientSound_->stop();
robot()->stopAllSounds();
alien()->stopAllSounds();
// Delete sound
delete ambientSound_;
ambientSound_ = 0;
escapePodBuildingPuzzle_.deinitialise();
sky_.deinitialise();
distantBackground_.deinitialise();
closeBackground_.deinitialise();
midground_.deinitialise();
playerStage_.deinitialise();
foreground_.deinitialise();
delete ending_;
}
int main(int argc, char *argv[])
{
// For handling Ctrl+C nicely
signal(SIGINT, signal_callback_handler);
// Create the Dimitri robot object connected
// to /dev/ttyUSB0
Dimitri robot(0, 4);
// Set low torque
robot.setMaxTorque(0.0);
// Control mode is off
robot.setControlMode(OFF);
// This array stores the pose angles
float pose[13];
// Main loop
while (true)
{
// Performs update
robot.update();
// Gets the pose angles
robot.getNormalizedPose( pose );
// Print the pose angles
for (int i = 0 ; i < 13 ; i++)
{
printf("%+01.3f,", pose[i]);
}
printf("\n");
}
return 0;
}
int robot(char str[6]) {
antal++;
if (strcmp(str,facit) == 0) {
return antal;
} else {
//drag b
if (???) { //Villkor för drag b?
char tmp = str[0];
str[0] = str[1];
str[1] = tmp;
}
//drag s
if (???) { //Villkor för drag s?
char tmp = str[4];
int i;
for (i=4; i>0; i--) {
str[i] = str[i-1];
}
str[0] = tmp;
}
robot(str);
}
}
void SensorPointXY::sense() {
_robotPoseObject=0;
RobotType* r= dynamic_cast<RobotType*>(robot());
std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
int count = 0;
while (it!=r->trajectory().rend() && count < 1){
if (!_robotPoseObject)
_robotPoseObject = *it;
it++;
count++;
}
for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin();
it!=world()->objects().end(); it++){
WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
if (o && isVisible(o)){
EdgeType* e=mkEdge(o);
if (e && graph()) {
e->setMeasurementFromState();
addNoise(e);
graph()->addEdge(e);
}
}
}
}
void ShipBuildingArea::render()
{
sky_.render();
distantBackground_.render();
closeBackground_.render();
midground_.render();
playerStage_.render();
if(secondPass_)
{
escapePodBuildingPuzzle_.render();
}
if(!secondPass_)
{
// Render the robot
robot()->render();
}
// Render alien
alien()->render();
foreground_.render();
}
int main(int argc, char** argv)
{
mrcoreInit();
AriaBase robot(argc,argv);
WheeledBaseServer server(&robot,"base");
LaserSensorServer laserServer(&robot,"laser");
server.init(13000,1,true);
laserServer.init(13001,1,true);
while(1)
{
Sleep(500);
robot.watchdog();
LaserData data;
robot.getData(data);
cout<<"MEdidas: "<<data.getRanges().size()<<endl;
if(data.getRanges().size()>0)
cout<<"Medida: "<<data.getRanges()[data.getRanges().size()/2]<<endl;
}
server.close();
LOG_INFO("End of server");
return 1;
}
void event::actualize()
{
switch( type ) {
case EVENT_HELP:
debugmsg("Currently disabled while NPC and monster factions are being rewritten.");
/*
{
int num = 1;
if( faction_id >= 0 ) {
num = rng( 1, 6 );
}
for( int i = 0; i < num; i++ ) {
npc *temp = new npc();
temp->normalize();
if( faction_id != -1 ) {
faction *fac = g->faction_by_id( faction_id );
if( fac ) {
temp->randomize_from_faction( fac );
} else {
debugmsg( "EVENT_HELP run with invalid faction_id" );
temp->randomize();
}
} else {
temp->randomize();
}
temp->attitude = NPCATT_DEFEND;
// important: npc::spawn_at must be called to put the npc into the overmap
temp->spawn_at( g->get_abs_levx(), g->get_abs_levy(), g->get_abs_levz() );
// spawn at the border of the reality bubble, outside of the players view
if( one_in( 2 ) ) {
temp->posx = rng( 0, SEEX * MAPSIZE - 1 );
temp->posy = rng( 0, 1 ) * SEEY * MAPSIZE;
} else {
temp->posx = rng( 0, 1 ) * SEEX * MAPSIZE;
temp->posy = rng( 0, SEEY * MAPSIZE - 1 );
}
// And tell the npc to go to the player.
temp->goal.x = g->om_global_location().x;
temp->goal.y = g->om_global_location().y;
// The npcs will be loaded later by game::load_npcs()
}
}
*/
break;
case EVENT_ROBOT_ATTACK: {
if (rl_dist(g->get_abs_levx(), g->get_abs_levy(), map_point.x, map_point.y) <= 4) {
mtype *robot_type = GetMType("mon_tripod");
if (faction_id == 0) { // The cops!
if (one_in(2)) {
robot_type = GetMType("mon_copbot");
} else {
robot_type = GetMType("mon_riotbot");
}
g->u.add_memorial_log(pgettext("memorial_male", "Became wanted by the police!"),
pgettext("memorial_female", "Became wanted by the police!"));
}
monster robot(robot_type);
int robx = (g->get_abs_levx() > map_point.x ? 0 - SEEX * 2 : SEEX * 4),
roby = (g->get_abs_levy() > map_point.y ? 0 - SEEY * 2 : SEEY * 4);
robot.spawn(robx, roby);
g->add_zombie(robot);
}
} break;
case EVENT_SPAWN_WYRMS: {
if (g->levz >= 0)
return;
g->u.add_memorial_log(pgettext("memorial_male", "Awoke a group of dark wyrms!"),
pgettext("memorial_female", "Awoke a group of dark wyrms!"));
monster wyrm(GetMType("mon_dark_wyrm"));
int num_wyrms = rng(1, 4);
for (int i = 0; i < num_wyrms; i++) {
int tries = 0;
int monx = -1, mony = -1;
do {
monx = rng(0, SEEX * MAPSIZE);
mony = rng(0, SEEY * MAPSIZE);
tries++;
} while (tries < 10 && !g->is_empty(monx, mony) &&
rl_dist(g->u.posx, g->u.posy, monx, mony) <= 2);
if (tries < 10) {
wyrm.spawn(monx, mony);
g->add_zombie(wyrm);
}
}
if (!one_in(25)) // They just keep coming!
g->add_event(EVENT_SPAWN_WYRMS, int(calendar::turn) + rng(15, 25));
} break;
case EVENT_AMIGARA: {
g->u.add_memorial_log(pgettext("memorial_male", "Angered a group of amigara horrors!"),
pgettext("memorial_female", "Angered a group of amigara horrors!"));
int num_horrors = rng(3, 5);
int faultx = -1, faulty = -1;
bool horizontal = false;
for (int x = 0; x < SEEX * MAPSIZE && faultx == -1; x++) {
for (int y = 0; y < SEEY * MAPSIZE && faulty == -1; y++) {
if (g->m.ter(x, y) == t_fault) {
faultx = x;
faulty = y;
if (g->m.ter(x - 1, y) == t_fault || g->m.ter(x + 1, y) == t_fault)
horizontal = true;
else
horizontal = false;
}
}
}
monster horror(GetMType("mon_amigara_horror"));
for (int i = 0; i < num_horrors; i++) {
int tries = 0;
int monx = -1, mony = -1;
do {
if (horizontal) {
monx = rng(faultx, faultx + 2 * SEEX - 8);
for (int n = -1; n <= 1; n++) {
if (g->m.ter(monx, faulty + n) == t_rock_floor)
mony = faulty + n;
}
} else { // Vertical fault
mony = rng(faulty, faulty + 2 * SEEY - 8);
for (int n = -1; n <= 1; n++) {
if (g->m.ter(faultx + n, mony) == t_rock_floor)
monx = faultx + n;
}
}
tries++;
} while ((monx == -1 || mony == -1 || g->is_empty(monx, mony)) &&
tries < 10);
if (tries < 10) {
horror.spawn(monx, mony);
g->add_zombie(horror);
}
}
} break;
case EVENT_ROOTS_DIE:
g->u.add_memorial_log(pgettext("memorial_male", "Destroyed a triffid grove."),
pgettext("memorial_female", "Destroyed a triffid grove."));
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_root_wall && one_in(3))
g->m.ter_set(x, y, t_underbrush);
}
}
break;
case EVENT_TEMPLE_OPEN: {
g->u.add_memorial_log(pgettext("memorial_male", "Opened a strange temple."),
pgettext("memorial_female", "Opened a strange temple."));
bool saw_grate = false;
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_grate) {
g->m.ter_set(x, y, t_stairs_down);
if (!saw_grate && g->u_see(x, y))
saw_grate = true;
}
}
}
if (saw_grate)
add_msg(_("The nearby grates open to reveal a staircase!"));
} break;
case EVENT_TEMPLE_FLOOD: {
bool flooded = false;
ter_id flood_buf[SEEX*MAPSIZE][SEEY*MAPSIZE];
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++)
flood_buf[x][y] = g->m.ter(x, y);
}
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_water_sh) {
bool deepen = false;
for (int wx = x - 1; wx <= x + 1 && !deepen; wx++) {
for (int wy = y - 1; wy <= y + 1 && !deepen; wy++) {
if (g->m.ter(wx, wy) == t_water_dp)
deepen = true;
}
}
if (deepen) {
flood_buf[x][y] = t_water_dp;
flooded = true;
}
} else if (g->m.ter(x, y) == t_rock_floor) {
bool flood = false;
for (int wx = x - 1; wx <= x + 1 && !flood; wx++) {
for (int wy = y - 1; wy <= y + 1 && !flood; wy++) {
if (g->m.ter(wx, wy) == t_water_dp || g->m.ter(wx, wy) == t_water_sh)
flood = true;
}
}
if (flood) {
flood_buf[x][y] = t_water_sh;
flooded = true;
}
}
}
}
if (!flooded)
return; // We finished flooding the entire chamber!
// Check if we should print a message
if (flood_buf[g->u.posx][g->u.posy] != g->m.ter(g->u.posx, g->u.posy)) {
if (flood_buf[g->u.posx][g->u.posy] == t_water_sh) {
add_msg(m_warning, _("Water quickly floods up to your knees."));
g->u.add_memorial_log(pgettext("memorial_male", "Water level reached knees."),
pgettext("memorial_female", "Water level reached knees."));
} else { // Must be deep water!
add_msg(m_warning, _("Water fills nearly to the ceiling!"));
g->u.add_memorial_log(pgettext("memorial_male", "Water level reached the ceiling."),
pgettext("memorial_female", "Water level reached the ceiling."));
g->plswim(g->u.posx, g->u.posy);
}
}
// flood_buf is filled with correct tiles; now copy them back to g->m
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++)
g->m.ter_set(x, y, flood_buf[x][y]);
}
g->add_event(EVENT_TEMPLE_FLOOD, int(calendar::turn) + rng(2, 3));
} break;
case EVENT_TEMPLE_SPAWN: {
std::string montype = "mon_null";
switch (rng(1, 4)) {
case 1: montype = "mon_sewer_snake"; break;
case 2: montype = "mon_centipede"; break;
case 3: montype = "mon_dermatik"; break;
case 4: montype = "mon_spider_widow_giant"; break;
}
monster spawned( GetMType(montype) );
int tries = 0, x, y;
do {
x = rng(g->u.posx - 5, g->u.posx + 5);
y = rng(g->u.posy - 5, g->u.posy + 5);
tries++;
} while (tries < 20 && !g->is_empty(x, y) &&
rl_dist(x, y, g->u.posx, g->u.posy) <= 2);
if (tries < 20) {
spawned.spawn(x, y);
g->add_zombie(spawned);
}
} break;
default:
break; // Nothing happens for other events
}
}
int
main(int argc, const char ** argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
exit (-1);
}
// We open two displays, one for the internal camera view, the other one for
// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt;
#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt;
#endif
vpImage<unsigned char> Iint(480, 640, 255);
if (opt_display) {
// open a display for the visualization
displayInt.init(Iint,700,0, "Internal view") ;
}
vpServo task;
std::cout << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, articular velocity are computed"
<< std::endl ;
std::cout << " Simulation " << std::endl ;
std::cout << " task : servo 4 points " << std::endl ;
std::cout << "----------------------------------------------" << std::endl ;
std::cout << std::endl ;
// sets the initial camera location
vpHomogeneousMatrix cMo(-0.05,-0.05,0.7,
vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
// sets the point coordinates in the object frame
vpPoint point[4] ;
point[0].setWorldCoordinates(-0.045,-0.045,0) ;
point[3].setWorldCoordinates(-0.045,0.045,0) ;
point[2].setWorldCoordinates(0.045,0.045,0) ;
point[1].setWorldCoordinates(0.045,-0.045,0) ;
// computes the point coordinates in the camera frame and its 2D coordinates
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cMo) ;
// sets the desired position of the point
vpFeaturePoint p[4] ;
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i],point[i]) ; //retrieve x,y and Z of the vpPoint structure
// sets the desired position of the feature point s*
vpFeaturePoint pd[4] ;
// Desired pose
vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,0.8,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)));
// Projection of the points
for (unsigned int i = 0 ; i < 4 ; i++)
point[i].track(cdMo);
for (unsigned int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i], point[i]);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED) ;
// we want to see a point on a point
for (unsigned int i = 0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// set the gain
task.setLambda(0.8) ;
// Declaration of the robot
vpSimulatorAfma6 robot(opt_display);
// Initialise the robot and especially the camera
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Initialise the object for the display part*/
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
// Initialise the position of the object relative to the pose of the robot's camera
robot.initialiseObjectRelativeToCamera(cMo);
// Set the desired position (for the displaypart)
robot.setDesiredCameraPosition(cdMo);
// Get the internal robot's camera parameters
vpCameraParameters cam;
robot.getCameraParameters(cam,Iint);
if (opt_display)
{
//Get the internal view
vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
// Display task information
task.print() ;
unsigned int iter=0 ;
vpTRACE("\t loop") ;
while(iter++<500)
{
std::cout << "---------------------------------------------" << iter <<std::endl ;
vpColVector v ;
// Get the Time at the beginning of the loop
double t = vpTime::measureTimeMs();
// Get the current pose of the camera
cMo = robot.get_cMo();
if (iter==1) {
std::cout <<"Initial robot position with respect to the object frame:\n";
cMo.print();
}
// new point position
for (unsigned int i = 0 ; i < 4 ; i++)
{
point[i].track(cMo) ;
// retrieve x,y and Z of the vpPoint structure
vpFeatureBuilder::create(p[i],point[i]) ;
}
if (opt_display)
{
// Get the internal view and display it
vpDisplay::display(Iint) ;
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
if (opt_display && opt_click_allowed && iter == 1)
{
// suppressed for automate test
std::cout << "Click in the internal view window to continue..." << std::endl;
vpDisplay::getClick(Iint) ;
}
// compute the control law
v = task.computeControlLaw() ;
// send the camera velocity to the controller
robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
std::cout << "|| s - s* || " << ( task.getError() ).sumSquare() <<std::endl ;
// The main loop has a duration of 10 ms at minimum
vpTime::wait(t,10);
}
// Display task information
task.print() ;
task.kill();
std::cout <<"Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed)
{
// suppressed for automate test
std::cout << "Click in the internal view window to end..." << std::endl;
vpDisplay::getClick(Iint) ;
}
return 0;
}
catch(vpException e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
int main(int argc, char **argv)
{
std::string str;
int ret;
time_t lastTime;
int trans, rot;
ArJoyHandler joyHandler;
ArSerialConnection con;
ArRobot robot(NULL, false);
joyHandler.init();
joyHandler.setSpeeds(100, 700);
if (joyHandler.haveJoystick())
{
printf("Have a joystick\n\n");
}
else
{
printf("Do not have a joystick, set up the joystick then rerun the program\n\n");
// exit(0);
}
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
exit(0);
}
robot.setDeviceConnection(&con);
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
exit(0);
}
robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::ENABLE, 1);
robot.comInt(ArCommands::SOUNDTOG, 0);
//robot.comInt(ArCommands::ENCODER, 1);
//robot.comStrN(ArCommands::SAY, "\1\6\2\105", 4);
lastTime = time(NULL);
while (1)
{
if (!robot.isConnected())
{
printf("No longer connected to robot, exiting.\n");
exit(0);
}
robot.loopOnce();
if (lastTime != time(NULL))
{
printf("\rx %6.1f y %6.1f tth %6.1f vel %7.1f mpacs %3d", robot.getX(),
robot.getY(), robot.getTh(), robot.getVel(),
robot.getMotorPacCount());
fflush(stdout);
lastTime = time(NULL);
}
if (joyHandler.haveJoystick() && (joyHandler.getButton(1) ||
joyHandler.getButton(2)))
{
if (ArMath::fabs(robot.getVel()) < 10.0)
robot.comInt(ArCommands::ENABLE, 1);
joyHandler.getAdjusted(&rot, &trans);
robot.comInt(ArCommands::VEL, trans);
robot.comInt(ArCommands::RVEL, -rot);
}
else
{
robot.comInt(ArCommands::VEL, 0);
robot.comInt(ArCommands::RVEL, 0);
}
ArUtil::sleep(100);
}
}
void display(void) {
glClear(GL_COLOR_BUFFER_BIT);
robot(torso_angle, head_angle);
glutSwapBuffers();
}
int main()
{
#ifdef _TEST1
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
//Print loaded data
robot.PrintHomogenTransformationMatrix();
robot.PrintFullTransformationMatrix();
//Rotating joint 1 for 90 degrees
robot.RotateJoint(DHINDEX(1),90.0f);
robot.PrintHomogenTransformationMatrix();
robot.PrintFullTransformationMatrix();
#endif
#ifdef _TEST2
CLine3D line;
Vector3f test2;
Vector3f vec_end(20,1,0);
Vector3f vec_start(1,2,3);
float displ = 0.01f;
test2 = line.GetNextPoint(vec_end,vec_start,displ);
while(true)
{
if (test2 == vec_end)
{
break;
}
std::cout<<test2<<std::endl;
test2 = line.GetNextPoint(vec_end,test2,displ);
}
std::cout<<test2<<std::endl;
#endif
#ifdef _TESTJACIBIANTRANSPOSE
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f ,200.0f ,0.0f ,0.0f ,0.0f ,0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(JACOBIANTRANSPOSE,des,robot);
pJpt->SetAdditionalParametr(speccfc);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
#ifdef _TESTJACIBIANPSEUDO
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f , 200.0f , 0.0f , 0.0f , 0.0f , 0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(JACOBIANPSEVDOINVERSE,des,robot);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
#ifdef _TESTDLS
std::string str = "robot.xml";
CConfigLoader cfg(str);
if(!cfg.LoadXml()) return 1;
//Set origin at O_zero
CRobot robot(Vector3f(0.0f,0.0f,0.0f));
robot.LoadConfig(cfg.GetTable());
CAlgoFactory factory;
VectorXf des(6);
float speccfc = 0.001f;
des << 200.0f , 200.0f , 0.0f , 0.0f , 0.0f , 0.0f ;
CAlgoAbstract * pJpt = factory.GiveMeSolver(DUMPEDLEASTSQUARES,des,robot);
pJpt->SetAdditionalParametr(speccfc);
pJpt->CalculateData();
robot.PrintConfiguration();
#endif
return 0;
}
void event::actualize(game *g)
{
switch (type) {
case EVENT_HELP: {
npc tmp;
int num = 1;
if (faction_id >= 0)
num = rng(1, 6);
for (int i = 0; i < num; i++) {
if (faction_id != -1) {
faction* fac = g->faction_by_id(faction_id);
if (fac)
tmp.randomize_from_faction(g, fac);
else
debugmsg("EVENT_HELP run with invalid faction_id");
} else
tmp.randomize(g);
tmp.attitude = NPCATT_DEFEND;
tmp.posx = g->u.posx - SEEX * 2 + rng(-5, 5);
tmp.posy = g->u.posy - SEEY * 2 + rng(-5, 5);
g->active_npc.push_back(&tmp);
}
} break;
case EVENT_ROBOT_ATTACK: {
if (rl_dist(g->levx, g->levy, map_point.x, map_point.y) <= 4) {
mtype *robot_type = GetMType("mon_tripod");
if (faction_id == 0) { // The cops!
robot_type = GetMType("mon_copbot");
g->u.add_memorial_log(_("Became wanted by the police!"));
}
monster robot(robot_type);
int robx = (g->levx > map_point.x ? 0 - SEEX * 2 : SEEX * 4),
roby = (g->levy > map_point.y ? 0 - SEEY * 2 : SEEY * 4);
robot.spawn(robx, roby);
g->add_zombie(robot);
}
} break;
case EVENT_SPAWN_WYRMS: {
if (g->levz >= 0)
return;
g->u.add_memorial_log(_("Awoke a group of dark wyrms!"));
monster wyrm(GetMType("mon_dark_wyrm"));
int num_wyrms = rng(1, 4);
for (int i = 0; i < num_wyrms; i++) {
int tries = 0;
int monx = -1, mony = -1;
do {
monx = rng(0, SEEX * MAPSIZE);
mony = rng(0, SEEY * MAPSIZE);
tries++;
} while (tries < 10 && !g->is_empty(monx, mony) &&
rl_dist(g->u.posx, g->u.posx, monx, mony) <= 2);
if (tries < 10) {
wyrm.spawn(monx, mony);
g->add_zombie(wyrm);
}
}
if (!one_in(25)) // They just keep coming!
g->add_event(EVENT_SPAWN_WYRMS, int(g->turn) + rng(15, 25));
} break;
case EVENT_AMIGARA: {
g->u.add_memorial_log(_("Angered a group of amigara horrors!"));
int num_horrors = rng(3, 5);
int faultx = -1, faulty = -1;
bool horizontal = false;
for (int x = 0; x < SEEX * MAPSIZE && faultx == -1; x++) {
for (int y = 0; y < SEEY * MAPSIZE && faulty == -1; y++) {
if (g->m.ter(x, y) == t_fault) {
faultx = x;
faulty = y;
if (g->m.ter(x - 1, y) == t_fault || g->m.ter(x + 1, y) == t_fault)
horizontal = true;
else
horizontal = false;
}
}
}
monster horror(GetMType("mon_amigara_horror"));
for (int i = 0; i < num_horrors; i++) {
int tries = 0;
int monx = -1, mony = -1;
do {
if (horizontal) {
monx = rng(faultx, faultx + 2 * SEEX - 8);
for (int n = -1; n <= 1; n++) {
if (g->m.ter(monx, faulty + n) == t_rock_floor)
mony = faulty + n;
}
} else { // Vertical fault
mony = rng(faulty, faulty + 2 * SEEY - 8);
for (int n = -1; n <= 1; n++) {
if (g->m.ter(faultx + n, mony) == t_rock_floor)
monx = faultx + n;
}
}
tries++;
} while ((monx == -1 || mony == -1 || g->is_empty(monx, mony)) &&
tries < 10);
if (tries < 10) {
horror.spawn(monx, mony);
g->add_zombie(horror);
}
}
} break;
case EVENT_ROOTS_DIE:
g->u.add_memorial_log(_("Destroyed a triffid grove."));
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_root_wall && one_in(3))
g->m.ter_set(x, y, t_underbrush);
}
}
break;
case EVENT_TEMPLE_OPEN: {
g->u.add_memorial_log(_("Opened a strange temple."));
bool saw_grate = false;
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_grate) {
g->m.ter_set(x, y, t_stairs_down);
if (!saw_grate && g->u_see(x, y))
saw_grate = true;
}
}
}
if (saw_grate)
g->add_msg(_("The nearby grates open to reveal a staircase!"));
} break;
case EVENT_TEMPLE_FLOOD: {
bool flooded = false;
ter_id flood_buf[SEEX*MAPSIZE][SEEY*MAPSIZE];
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++)
flood_buf[x][y] = g->m.ter(x, y);
}
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++) {
if (g->m.ter(x, y) == t_water_sh) {
bool deepen = false;
for (int wx = x - 1; wx <= x + 1 && !deepen; wx++) {
for (int wy = y - 1; wy <= y + 1 && !deepen; wy++) {
if (g->m.ter(wx, wy) == t_water_dp)
deepen = true;
}
}
if (deepen) {
flood_buf[x][y] = t_water_dp;
flooded = true;
}
} else if (g->m.ter(x, y) == t_rock_floor) {
bool flood = false;
for (int wx = x - 1; wx <= x + 1 && !flood; wx++) {
for (int wy = y - 1; wy <= y + 1 && !flood; wy++) {
if (g->m.ter(wx, wy) == t_water_dp || g->m.ter(wx, wy) == t_water_sh)
flood = true;
}
}
if (flood) {
flood_buf[x][y] = t_water_sh;
flooded = true;
}
}
}
}
if (!flooded)
return; // We finished flooding the entire chamber!
// Check if we should print a message
if (flood_buf[g->u.posx][g->u.posy] != g->m.ter(g->u.posx, g->u.posy)) {
if (flood_buf[g->u.posx][g->u.posy] == t_water_sh) {
g->add_msg(_("Water quickly floods up to your knees."));
g->u.add_memorial_log(_("Water level reached knees."));
} else { // Must be deep water!
g->add_msg(_("Water fills nearly to the ceiling!"));
g->u.add_memorial_log(_("Water level reached the ceiling."));
g->plswim(g->u.posx, g->u.posy);
}
}
// flood_buf is filled with correct tiles; now copy them back to g->m
for (int x = 0; x < SEEX * MAPSIZE; x++) {
for (int y = 0; y < SEEY * MAPSIZE; y++)
g->m.ter_set(x, y, flood_buf[x][y]);
}
g->add_event(EVENT_TEMPLE_FLOOD, int(g->turn) + rng(2, 3));
} break;
case EVENT_TEMPLE_SPAWN: {
std::string montype = "mon_null";
switch (rng(1, 4)) {
case 1: montype = "mon_sewer_snake"; break;
case 2: montype = "mon_centipede"; break;
case 3: montype = "mon_dermatik"; break;
case 4: montype = "mon_spider_widow"; break;
}
monster spawned( GetMType(montype) );
int tries = 0, x, y;
do {
x = rng(g->u.posx - 5, g->u.posx + 5);
y = rng(g->u.posy - 5, g->u.posy + 5);
tries++;
} while (tries < 20 && !g->is_empty(x, y) &&
rl_dist(x, y, g->u.posx, g->u.posy) <= 2);
if (tries < 20) {
spawned.spawn(x, y);
g->add_zombie(spawned);
}
} break;
default:
break; // Nothing happens for other events
}
}
