int
dolongcmd(char *buffer, struct value *arg, int narg)
{
if (cx_beginbuf(buffer, arg, narg) < 0)
return -1;
p_start();
err_end();
cx_end();
return 0;
}
int
dosource(char *filename)
{
if (cx_beginfile(filename) < 0)
return -1;
p_start();
err_end();
cx_end();
return 0;
}
bool ArmControlCartesian::setTargetAngular(double x, double y, double z, double r, double p, double w, double time)
{
if( !isEmpty() )
return false;
//
// set command time
//
setCmdTime(m_assignedPartIndex);
//
// position
//
hrp::Vector3 p_start(m_jointPath->endLink()->p);
hrp::Vector3 p_end(x,y,z);
if( !m_pos_interpolator->calcInterpolation(&p_start(0), &p_end(0), time) ) {
std::cout << m_instanceName << " : calc pos interpolation error" << std::endl;
return false;
}
//
// attitude
//
hrp::Matrix33 R_start(m_jointPath->endLink()->attitude());
hrp::Matrix33 R_end(hrp::rotFromRpy(r,p,w));
if( !m_rot_interpolator->calcInterpolation(R_start, R_end, hrp::Vector3(0,0,-1), time) ) {
m_pos_interpolator->clear();
std::cout << m_instanceName << " : calc rot interpolation error" << std::endl;
return false;
}
//std::cout << m_instanceName << " : reference attitude = " << R_end << std::endl;
return true;
}
float GPS_Filter::getDistanceHeading(const GPS_Info &info)
{
LONG prj_x, prj_y;
LONG last_x, last_y;
GlobalCrt crt;
crt.geoToPrj(info.longitude, info.latitude, &prj_x, &prj_y);
crt.geoToPrj(m_last_used.longitude, m_last_used.latitude, &last_x, &last_y);
Pos2DEx p_start(last_x, last_y);
Pos2DEx p_end(prj_x, prj_y);
double angle = SpatialMath::calLineNAngle(p_start, p_end);
float heading = angle / PI * 180.0;
return heading;
}
/* create a biarc for a bezier curve.
*
* extends the tangent lines to the bezier curve at its first and last control
* points, and intersects them to find a third point.
* the biarc passes through the first and last control points, and the incenter
* of the circle defined by the first, last and intersection points.
*/
HIDDEN ON_Arc
make_biarc(const ON_BezierCurve& bezier)
{
ON_2dPoint isect, arc_pt;
ON_2dPoint p_start(bezier.PointAt(0)), p_end(bezier.PointAt(1.0));
ON_2dVector t_start(bezier.TangentAt(0)), t_end(bezier.TangentAt(1.0));
ON_Ray r_start(p_start, t_start), r_end(p_end, t_end);
r_start.IntersectRay(r_end, isect);
arc_pt = incenter(p_start, p_end, isect);
return ON_Arc(p_start, arc_pt, p_end);
}
bool
SSLPathPlanner::isPlanDone (uint8_t id)
{
Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y);
Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y);
if (getSquaredDistance (p_start, p_goal) < POSE_REACHED_DIST)
{
is_plan_done[id] = true;
return true;
}
else
return false;
}
int parse(const char *pttfile, const struct pt_config *conf)
{
int errcode;
struct parser *p;
p = p_alloc(pttfile, conf);
if (!p)
return -err_no_mem;
errcode = p_open_files(p);
if (errcode < 0)
goto error;
errcode = p_start(p);
p_close_files(p);
error:
p_free(p);
return errcode;
}
static bool
test_route(const unsigned n_airspaces, const RasterMap& map)
{
Airspaces airspaces;
setup_airspaces(airspaces, map.GetMapCenter(), n_airspaces);
{
std::ofstream fout("results/terrain.txt");
unsigned nx = 100;
unsigned ny = 100;
GeoPoint origin(map.GetMapCenter());
for (unsigned i = 0; i < nx; ++i) {
for (unsigned j = 0; j < ny; ++j) {
fixed fx = (fixed)i / (nx - 1) * fixed(2.0) - fixed_one;
fixed fy = (fixed)j / (ny - 1) * fixed(2.0) - fixed_one;
GeoPoint x(origin.longitude + Angle::Degrees(fixed(0.2) + fixed(0.7) * fx),
origin.latitude + Angle::Degrees(fixed(0.9) * fy));
short h = map.GetInterpolatedHeight(x);
fout << x.longitude.Degrees() << " " << x.latitude.Degrees()
<< " " << h << "\n";
}
fout << "\n";
}
fout << "\n";
}
{
// local scope, see what happens when we go out of scope
GeoPoint p_start(Angle::Degrees(fixed(-0.3)), Angle::Degrees(fixed(0.0)));
p_start += map.GetMapCenter();
GeoPoint p_dest(Angle::Degrees(fixed(0.8)), Angle::Degrees(fixed(-0.7)));
p_dest += map.GetMapCenter();
AGeoPoint loc_start(p_start, RoughAltitude(map.GetHeight(p_start) + 100));
AGeoPoint loc_end(p_dest, RoughAltitude(map.GetHeight(p_dest) + 100));
AircraftState state;
GlidePolar glide_polar(fixed(0.1));
AirspaceAircraftPerformanceGlide perf(glide_polar);
GeoVector vec(loc_start, loc_end);
fixed range = fixed(10000) + vec.distance / 2;
state.location = loc_start;
state.altitude = loc_start.altitude;
{
Airspaces as_route(airspaces, false);
// dummy
// real one, see if items changed
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
int size_1 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_1);
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), fixed_one);
int size_2 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_2);
ok(size_2 < size_1, "shrink as", 0);
// go back
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_end), range);
int size_3 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_3);
ok(size_3 >= size_2, "grow as", 0);
// and again
as_route.synchronise_in_range(airspaces, vec.MidPoint(loc_start), range);
int size_4 = as_route.size();
if (verbose)
printf("# route airspace size %d\n", size_4);
ok(size_4 >= size_3, "grow as", 0);
scan_airspaces(state, as_route, perf, true, loc_end);
}
// try the solver
SpeedVector wind(Angle::Degrees(fixed(0)), fixed(0.0));
GlidePolar polar(fixed_one);
GlideSettings settings;
settings.SetDefaults();
AirspaceRoute route(airspaces);
route.UpdatePolar(settings, polar, polar, wind);
route.SetTerrain(&map);
RoutePlannerConfig config;
config.mode = RoutePlannerConfig::Mode::BOTH;
bool sol = false;
for (int i = 0; i < NUM_SOL; i++) {
loc_end.latitude += Angle::Degrees(fixed(0.1));
loc_end.altitude = map.GetHeight(loc_end) + 100;
route.Synchronise(airspaces, loc_start, loc_end);
if (route.Solve(loc_start, loc_end, config)) {
sol = true;
if (verbose) {
PrintHelper::print_route(route);
}
} else {
if (verbose) {
printf("# fail\n");
}
sol = false;
}
char buffer[80];
sprintf(buffer, "route %d solution", i);
ok(sol, buffer, 0);
}
}
return true;
}
bool PageIterator::IsWithinFirstTextlineOfParagraph() const {
PageIterator p_start(*this);
p_start.RestartParagraph();
return p_start.it_->row() == it_->row();
}
int main(int argc, char * argv[])
{
void b2grandpwr(void);
void s2grandpwr(void);
void b2imaxx(void);
void b2huake(void);
void p_line(void);
void p_start();
void yosunpi(void);
/*
* progam format: mypi pihead billto shipto payment itemsnumber partnumber rsl qty
* mypi yosun b2globalic s2globalic TT 2 ADS8320E/2K5 3.00 5,000 ADS8345BN 6.00 2,000
* PI head
* 1 yosun
* 2 globalic
*/
int pihead;
int b2addr;
int s2addr;
/*
* check pihead
*/
if(argv[1]=="yosun")
pihead=1;
else if(argv[1]=="globalic")
pihead=2;
else
pihead=0;
/*
* check bill to address
*/
if(argv[2]=="b2grandpwr")
b2addr=1;
else if(argv[2]=="imaxx")
b2addr=2;
else if(argv[2]=="huake")
b2addr=3;
else if(argv[2]=="fuyang")
b2addr=4;
else if(argv[2]=="sense")
b2addr=5;
else if(argv[2]=="ereach")
b2addr=6;
else
b2addr=0;
/*
* check ship to address
*/
if(argv[2]=="b2grandpwr")
s2addr=1;
else if(argv[2]=="imaxx")
s2addr=2;
else if(argv[2]=="huake")
s2addr=3;
else if(argv[2]=="fuyang")
s2addr=4;
else if(argv[2]=="sense")
s2addr=5;
else if(argv[2]=="ereach")
s2addr=6;
else
s2addr=0;
p_line();
b2grandpwr();
s2grandpwr();
b2imaxx();
b2huake();
p_start();
//printf("test\n");
return 0;
}
bool
SSLPathPlanner::doPlanForRobot (const int& id/*, bool& is_stop*/)
{
// is_send_plan = true;
tmp_robot_id_queried = id;
State* tmp_start = manifold->allocState ();
tmp_start->as<RealVectorStateManifold::StateType> ()->values[0] = team_state_[id].pose.x;
tmp_start->as<RealVectorStateManifold::StateType> ()->values[1] = team_state_[id].pose.y;
PathGeometric direct_path (planner_setup->getSpaceInformation ());
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[0], tmp_start);
State* tmp_goal = manifold->allocState ();
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[0] = pose_control.pose[id].pose.x;
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[1] = pose_control.pose[id].pose.y;
// std::cout<<pose_control.pose[id].pose.x<<"\t"<<pose_control.pose[id].pose.y<<std::endl;
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[1], tmp_goal);
Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y);
Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y);
//too close to the target point, no need for planning, just take it as a next_target_pose
if (sqrt (getSquaredDistance (p_start, p_goal)) < VERY_CRITICAL_DIST)
{
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
return true;
}
// manifold->printState (direct_path.states[0], std::cout);
// manifold->printState (direct_path.states[1], std::cout);
//direct path is available, no need for planning
// if (direct_path.check ())
if (!doesIntersectObstacles (id, p_start, p_goal))
{
// std::cout << "direct path is AVAILABLE for robot " << id << std::endl;
if (solution_data_for_robots[id].size () > direct_path.states.size ())
{
for (uint32_t i = direct_path.states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
solution_data_for_robots[id].resize (direct_path.states.size ());
}
else if (solution_data_for_robots[id].size () < direct_path.states.size ())
{
for (uint32_t i = solution_data_for_robots[id].size (); i < direct_path.states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
for (uint32_t i = 0; i < direct_path.states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], direct_path.states[i]);
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
// std::cout<<next_target_poses[id].x<<"\t"<<next_target_poses[id].y<<std::endl;
return true;
}
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
// std::cout << "direct path is NOT AVAILABLE for robot " << id << ", doing planning" << std::endl;
//direct path is not available, DO PLAN if the earlier plan is invalidated!
//earlier plan is still valid
if (checkPlanForRobot (id))
return true;
else if (is_plan_done[id])
return false;
//earlier plan is not valid anymore, replan
planner_setup->clear ();
planner_setup->clearStartStates ();
ScopedState<RealVectorStateManifold> start_state (manifold);
ScopedState<RealVectorStateManifold> goal_state (manifold);
start_state->values[0] = team_state_[id].pose.x;
start_state->values[1] = team_state_[id].pose.y;
goal_state->values[0] = pose_control.pose[id].pose.x;
goal_state->values[1] = pose_control.pose[id].pose.y;
planner_setup->setStartAndGoalStates (start_state, goal_state);
planner_setup->getProblemDefinition ()->fixInvalidInputStates (ssl::config::ROBOT_BOUNDING_RADIUS / 2.0,
ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, 100);
bool solved = planner_setup->solve (0.100);//100msec
if (solved)
{
planner_setup->simplifySolution ();
PathGeometric* path;
path = &planner_setup->getSolutionPath ();
// PathSimplifier p (planner_setup->getSpaceInformation ());
// p.reduceVertices (*path, 1000);
if (solution_data_for_robots[id].size () < path->states.size ())
{
for (unsigned int i = solution_data_for_robots[id].size (); i < path->states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
else if (solution_data_for_robots[id].size () > path->states.size ())
{
for (unsigned int i = path->states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
//drop last elements that are already being freed
solution_data_for_robots[id].resize (path->states.size ());
}
for (unsigned int i = 0; i < path->states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], path->states[i]);
//leader-follower approach based segment enlargement
//pick next location
Point_2 curr_point (path->states[0]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[0]->as<
RealVectorStateManifold::StateType> ()->values[1]);
for (uint32_t i = 1; i < path->states.size (); i++)
{
Point_2 next_point (path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[i]->as<
RealVectorStateManifold::StateType> ()->values[1]);
if (!doesIntersectObstacles (id, curr_point, next_point))
{
next_target_poses[id].x = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0];
next_target_poses[id].y = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[1];
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
}
else
break;
}
// next_target_poses[id].x = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[0];
// next_target_poses[id].y = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[1];
// planner_data_for_robots[id].clear ();
// planner_data_for_robots[id] = planner_setup->getPlannerData ();
}
return solved;
}
// --- main --------------------------------------------------------------------
int
mainloop (int argc, char *argv []) {
FILE *fp = NULL;
FILE *tempfp = NULL;
sql_stmt_t *stmts = NULL;
trace_t *t = NULL;
bool_t only_q = FALSE;
bool_t append = FALSE;
bool_t only_r = FALSE;
bool_t dbf_overwrite = FALSE;
char *filename = NULL;
char *folder = ".";
char *dt = NULL;
char *dt_filename = NULL;
unsigned long partition_interval = PARTITION_INTERVAL_SECS;
unsigned long partition_start = 0;
int opt_idx = 0;
int c = 0;
int rc = 0;
// command line option parsing
struct option long_opts [] = {
{"help", no_argument, NULL, 'h'},
{"version", no_argument, NULL, 'v'},
{"append", no_argument, NULL, 'a'},
{"show_schema", no_argument, NULL, 's'},
{"queries_only", no_argument, NULL, 'q'},
{"replies_only", no_argument, NULL, 'r'},
{"database", required_argument, NULL, 'd'},
{"db_overwrite", no_argument, NULL, 'o'},
{"interval", required_argument, NULL, 'i'},
{"db_folder", required_argument, NULL, 'f'},
{ NULL, 0, NULL, 0 }
};
while ((c = getopt_long (argc, argv, "hvasqrd:n:oi:f:", long_opts, &opt_idx)) != -1) {
switch (c) {
case 'v':
fprintf (stdout, "%s\n", DNS2SQLITE_VERSION); // VERSION defined in dns2sqlite.h
exit (0);
break;
case 's':
fprintf (stdout, "%s\n", g_tabledefs); // VERSION defined in dns2sqlite.h
exit (0);
break;
case 'a':
append = TRUE;
break;
case 'q':
only_q = TRUE;
break;
case 'r':
only_r = TRUE;
break;
case 'd':
filename = optarg;
break;
case 'o':
dbf_overwrite = TRUE;
break;
case 'i':
partition_interval = strtoul (optarg, NULL, NUM_BASE);
partition_interval = (partition_interval < 1 ? 1 : partition_interval) * 60;
break;
case 'f':
folder = optarg;
break;
default:
fprintf (stderr, "Unknown option: %c\n", c);
// FALL THRU
case 'h':
usage (argv [0]);
return 0;
}
}
argc -= optind;
argv += optind;
// handle dangling command line arguments
// N.B. this does not work if a pipe and a file is used at the same time.
// If files are given then they should be processed before the pipe is read
// from stdin. That is, while argc > 0 open each file and go to the main
// loop. When argc = 0 then open stdin and go to the main loop:
//
// while (argc > 0) {
// fp = fopen (argv [argc -1], "r");
// if (fp == NULL) {return FAILURE}
// else {read_file (fp, ...);}
// fclose (fp);
// argc--;
// }
// fp = stdin;
switch (argc) {
case 0:
fp = stdin;
break;
case 1:
fp = fopen (argv [argc -1], "r");
if (fp == NULL) {
perror (argv [argc -1]);
return 1;
}
break;
default:
usage (argv [0]);
return 1;
}
if (filename == NULL) {
fprintf (stderr, "Error: No database specified \n");
}
// main loop
// This should at least be moved into its own function.
while ((t = parse_line (fp)) != NULL) {
partition_start = p_start (partition_start, t->s, partition_interval);
if ((unsigned long) t->s >= (partition_start + partition_interval)) {
rc = commit ((stmts + COMMIT)->pstmt);
close_db (G_DB);
G_DB = NULL;
rc = chdir ("..");
if (rc == -1) {
perror (NULL);
return FAILURE;
}
partition_start += partition_interval;
}
// check if database is open
if (!isdbopen (G_DB)) {
// create directory name
if (dt)
XFREE(dt);
dt = sec_to_datetime_str (partition_start);
if (!dt)
return FAILURE;
// generate path
if (dt_filename)
XFREE(dt_filename);
dt_filename = make_dt_filename (dt, filename);
if (dt_filename == NULL) {
XFREE(dt);
return FAILURE;
}
// create directory
if (make_db_dir (dt, folder) != SUCCESS) {
XFREE(dt);
XFREE(dt_filename);
return FAILURE;
}
XFREE(dt);
if (dbf_overwrite)
{
rc = unlink(dt_filename);
if (rc==0)
d2log (LOG_ERR|LOG_USER, "Unlinked file %s (due to overwrite flag).",dt_filename);
else
{
rc = errno;
if (rc != ENOENT)
{
d2log (LOG_ERR|LOG_USER, "Failed to unlink %s (overwrite) file code %d.",dt_filename,rc);
XFREE(dt);
XFREE(dt_filename);
return FAILURE;
}
}
}
else
{
if (append == FALSE)
{
// simplistic test whether the database file exists
tempfp = fopen (dt_filename, "r");
if (tempfp) {
d2log (LOG_ERR|LOG_USER, "Error: Database file %s exists ! ( use -a to append or -o owerwrite ).",dt_filename);
fclose (tempfp);
return FAILURE;
}
}
}
if (!open_db (dt_filename, &G_DB, append)) {
d2log (LOG_ERR|LOG_USER, "Failed to create new db %s.",dt_filename);
XFREE(dt_filename);
return FAILURE;
}
if (!prepare_stmts (G_DB, &stmts)) {
d2log (LOG_ERR|LOG_USER, "Failed to prepare sql statements for db: %s.",dt_filename);
close_db (G_DB);
return FAILURE;
}
rc = start_transaction ((stmts + BEGIN_TRANS)->pstmt);
}
if (!store_to_db (G_DB, stmts, t, only_q, only_r)) {
d2log (LOG_ERR|LOG_USER, "Failed to store data to db.\n");
}
trace_free (t);
}
// clean up before exit
fclose (fp);
rc = commit ((stmts + COMMIT)->pstmt);
close_db (G_DB);
}
