JuceMenuPtr MLMenu::getJuceMenu()
{
buildIndex();
JuceMenuPtr jm(new PopupMenu());
mRoot->addToJuceMenu(mName.getString(), jm);
return jm;
}
int main(int argc, char ** argv) {
ros::init(argc, argv, "arm_gui");
ros::NodeHandle nh;
JointManager jm(nh);
jm.logicLoop();
}
vector<Vec>
JarvisMarch::
compute_convex_hull(const vector<Vec>& poly)
throw()
{
polygon_=poly;
size_of_polygon_=polygon_.size();
size_of_convex_hull_=0;
jm();
// size_of_convex_hull_ now contains the number of the first points in polygon_ (they are in the right order) which belong to the convex hull
vector<Vec> convex_hull;
for(int i=0;i<size_of_convex_hull_;i++)
{
convex_hull.push_back(polygon_[i]);
}
return convex_hull;
}
// next_movement = command_simulation_step -> scheduled "magically"
MovementInfo* TraCIClient::next_movement()
throw( std::runtime_error )
{
std::auto_ptr<MovementInfo> result(new MovementInfo());
if ( !socket_.get() )
{
std::cerr << "Error in method TraCIClient::next_movement, socket_ == NULL!" << std::endl;
abort();
}
const double now = sc_->world().scheduler().current_time();
// If in-storage is empty ask for new data from movement simulator (e.g. SUMO)
if ( nextStepVehicles.empty() )
{
// If the right simulation time for sending the next question is not yet arrived ...
if ( now < target_time_ )
{
//... wait until then
result->set_urgency( MovementInfo::Delayed );
result->set_dispatch_time( target_time_ );
result->set_node( NULL );
result->set_node_movement( NULL );
return result.release();
}
// If target_time is reached, ask for new movements
target_time_ = now + time_interval_;
check_for_unused_vehicle_nodes();
current_vehicle_nodes_.clear();
// send simulation step command
runSumo(target_time_);
// get new list of vehicles
command_get_value(id::CmdGetVehicleVariable, id::VarIdList, "", nextStepVehicles);
}
// no more vehicles on the road?
if ( nextStepVehicles.empty() )
{
// No equipped vehicle is reported
result->set_urgency( MovementInfo::Delayed );
result->set_dispatch_time( target_time_ );
result->set_node( NULL );
result->set_node_movement( NULL );
return result.release();
}
// handle movement of last new vehicle (last because that's an O(1) operation)
const std::string node_id = nextStepVehicles.back();
nextStepVehicles.pop_back();
// get new position
Position2DTypeWithTypeId newPosition;
command_get_value(id::CmdGetVehicleVariable, id::VarVehiclePosition, node_id, newPosition);
bool is_new_node = false;
Node * node = find_node_by_traci_id_w( TraCIID( DomainVehicle, node_id ) );
// If node doesn't exist create new one
if ( !node )
{
node = new_node( TraCIID( DomainVehicle, node_id ) );
is_new_node = true;
}
current_vehicle_nodes_.insert( TraCIID( DomainVehicle, node_id ) );
result->set_urgency( shawn::MovementInfo::Immediately );
result->set_dispatch_time( now );
result->set_node( node );
const Vec destination(newPosition.x, newPosition.y, 0.0);
if ( is_new_node )
{
// "Fresh" node
std::auto_ptr<JumpMovement> jm( new JumpMovement );
jm->set_dimensions(&destination);
result->set_node_movement( jm.release() );
}
else
{
// Old node
// Set node exactly to old destination
const LinearMovement* old_lm = dynamic_cast<const LinearMovement*>( &node->movement() );
if ( old_lm )
node->set_real_position( old_lm->destination() );
std::auto_ptr<LinearMovement> lm( new LinearMovement );
if ( target_time_ > now )
{
const double velocity = euclidean_distance( node->real_position(), destination ) / ( target_time_ - now );
lm->set_parameters( velocity, destination, sc_->world_w() );
}
result->set_node_movement( lm.release() );
}
return result.release();
}
void LSpace :: drawTriad(FloatArray &coords, int isurf)
{
FloatMatrix jm(3, 3);
FloatArray gc(3);
GraphicObj *go;
WCRec p [ 2 ]; // point
double coeff = 1.0;
int i, succ;
/*
* // version I
* this->interpolation.giveJacobianMatrixAt (jm, domain, nodeArray, coords);
* // determine origin
* this->interpolation.local2global (gc, domain, nodeArray, coords, 0.0);
* // draw triad
*
*/
// version II
// determine surface center
IntArray snodes(4);
FloatArray h1(3), h2(3), nn(3), n(3);
int j;
const char *colors[] = {
"red", "green", "blue"
};
this->interpolation.computeSurfaceMapping(snodes, dofManArray, isurf);
for ( i = 1; i <= 4; i++ ) {
gc.add( * ( domain->giveNode( snodes.at(i) )->giveCoordinates() ) );
}
gc.times(1. / 4.);
// determine "average normal"
nn.zero();
for ( i = 1; i <= 4; i++ ) {
j = ( i ) % 4 + 1;
h1 = * domain->giveNode( snodes.at(i) )->giveCoordinates();
h1.subtract(gc);
h2 = * domain->giveNode( snodes.at(j) )->giveCoordinates();
h2.subtract(gc);
n.beVectorProductOf(h1, h2);
if ( n.dotProduct(n, 3) > 1.e-6 ) {
n.normalize();
}
nn.add(n);
}
nn.times(1. / 4.);
if ( nn.dotProduct(nn, 3) < 1.e-6 ) {
return;
}
nn.normalize();
for ( i = 1; i <= 3; i++ ) {
jm.at(i, 3) = nn.at(i);
}
// determine lcs of surface
// local x axis in xy plane
double test = fabs(fabs( nn.at(3) ) - 1.0);
if ( test < 1.e-5 ) {
h1.at(1) = jm.at(1, 1) = 1.0;
h1.at(2) = jm.at(2, 1) = 0.0;
} else {
h1.at(1) = jm.at(1, 1) = jm.at(2, 3);
h1.at(2) = jm.at(2, 1) = -jm.at(1, 3);
}
h1.at(3) = jm.at(3, 1) = 0.0;
// local y axis perpendicular to local x,z axes
h2.beVectorProductOf(nn, h1);
for ( i = 1; i <= 3; i++ ) {
jm.at(i, 2) = h2.at(i);
}
p [ 0 ].x = gc.at(1);
p [ 0 ].y = gc.at(2);
p [ 0 ].z = gc.at(3);
for ( i = 1; i <= 3; i++ ) {
p [ 1 ].x = p [ 0 ].x + coeff *jm.at(1, i);
p [ 1 ].y = p [ 0 ].y + coeff *jm.at(2, i);
p [ 1 ].z = p [ 0 ].z + coeff *jm.at(3, i);
EASValsSetColor( ColorGetPixelFromString(const_cast< char * >(colors [ i - 1 ]), & succ) );
go = CreateLine3D(p);
EGWithMaskChangeAttributes(WIDTH_MASK | COLOR_MASK | LAYER_MASK, go);
EMAddGraphicsToModel(ESIModel(), go);
}
}
