int main(int argc, char** argv){
ros::init(argc, argv, "micros_swarm_framework_app1_node");
ros::NodeHandle nh;
int robot_id=-1;
bool param_ok = ros::param::get ("~unique_robot_id", robot_id);
if(!param_ok)
{
std::cout<<"could not get parameter unique_robot_id"<<std::endl;
exit(1);
}else{
std::cout<<"unique_robot_id = "<<robot_id<<std::endl;
}
micros_swarm_framework::KernelInitializer::initRobotID(robot_id);
ros::Subscriber sub = nh.subscribe("base_pose_ground_truth", 1000, baseCallback, ros::TransportHints().udp());
boost::thread barrier(&barrier_wait);
barrier.join();
boost::thread robot_move(&motion);
ros::spin();
return 0;
}
void robot_exec_cmd (robot_t * r)
{
char * cmd, * arg, * greeting;
int proc;
/* Does the name match? */
if (!(greeting = strtok(r->cmd, " ")) || strcmp(greeting, GREETING) != 0) {
robot_unknown_cmd(r);
return;
}
if (!(cmd = strtok(NULL, " "))) {
robot_unknown_cmd(r);
return;
}
/* MOVE */
if (strcmp(cmd, CMD_MOVE) == 0) {
robot_move(r);
}
/* LEFT */
else if (strcmp(cmd, CMD_LEFT) == 0) {
robot_rotate(r); /* TODO rename to robot_left */
}
/* REPAIR */
else if (strcmp(cmd, CMD_REPAIR) == 0) {
if (!(arg = strtok(NULL, " ")) || (sscanf(arg, "%d", &proc) != 1) || proc < 1 || proc > 9) {
robot_unknown_cmd(r);
return;
}
robot_repair(r, proc);
}
/* LIFT */
else if (strcmp(cmd, CMD_LIFT) == 0) {
robot_lift(r);
}
/* UNKNOWN */
else {
printf("Unknown command.\n");
robot_unknown_cmd(r);
}
}
int main(void)
{ port_init();
OledInit ();
timer1_init();
adc_init();
USART_Init(0x08);
_delay_ms(10);
while(1)
{
USART_Receive();//读取串口数据
Battery_voltage=adc_read(6)/102.40;
if (RX_Flag==1) //如果有新数据
{ RX_Flag=0;
TX_Data=RX_Data;
bb++;
USART_Transmit(TX_Data); //发送串口数据
}
rest(RX_Data);
if ((RX_Data==0x00)||(Battery_voltage<7.0))
{ off_servo();
if (Battery_voltage<7.0){DisplayChar_16X08(36,2,"~(zzz)~") ;}
else{DisplayChar_16X08(36,2,"~(@o@)~") ; }
}
if ((Battery_voltage>8.0)&&(!(RX_Data==0x00)))
{DisplayChar_16X08(36,2,"~(^o^)~") ;
}
else if ((Battery_voltage>7.0)&&(!(RX_Data==0x00)))
{DisplayChar_16X08(36,2,"~(=o=)~") ;
}
if (RX_Data==0x01)//站立
{ open_servo();
robot_stand(0,-90);
}
if (RX_Data==0x02)//左转
{robot_move(0,0,40,-90,90,100);}
if (RX_Data==0x03)//右转
{ robot_move(0,0,-40,-90,90,100);}
if (RX_Data==0x04)//前进
{ robot_move(0,40,0,-90,90,100);}
if (RX_Data==0x05)//后退
{robot_move(0,-40,0,-90,90,100);}
if (RX_Data==0x06)//
{robot_Sur_Place(1,-90,90,100);}
if (RX_Data==0x07)//
{robot_move(40,0,0,-90,90,100);}
if (RX_Data==0x08)//
{robot_move(40,0,0,-90,90,100);}
if (RX_Data==0x09)//
{robot_move(20,20,0,-90,90,100);}
//DisplayChar_16X08(36,2,"~(@o@)~") ;
Cache_MDigit5(RX_Data,6,0,1 );Cache_MDigit5(Battery_voltage*100,6,88,1 );
// OledDispPicture(0,32,64,512,Expression1);//爱心
// OledDispPicture(0,26,75,600,Expression2);//笑脸
//TODO:: Please write your application code
//time_seve++;
}
}
std::vector<std::vector<float>> Map::find_robot_moves(Map ©_map, node* N){
std::vector< std::vector<float> > cost_map(width, std::vector<float>(height) );
copy_map = *this;
for(auto n : N->diamonds){
copy_map.set(n, DIAMOND);
}
for(int x = 0; x < width; ++x){
for(int y = 0; y < height; ++y){
cost_map[x][y] = data[x][y];
}
}
for(auto n : N->diamonds){
cost_map[n.x][n.y] = DIAMOND;
}
float distance = 3; // this means distance is (color - 3)
std::priority_queue<robot_move,std::vector<robot_move>,robot_cost_comparator_functor> open_list;
std::queue<robot_move> surrounding_points;
cost_map[N->man.x][N->man.y] = distance;
if( N->parent == nullptr){
open_list.push(robot_move(distance, N->man, 'N'));
open_list.push(robot_move(distance, N->man, 'S'));
open_list.push(robot_move(distance, N->man, 'E'));
open_list.push(robot_move(distance, N->man, 'W'));
} else {
for(size_t i = 0; i < N->diamonds.size(); ++i){
if(N->man == N->parent->diamonds.at(i) ){
pos_t displacement = N->diamonds.at(i) - N->parent->diamonds.at(i);
if( displacement == above){ open_list.push(robot_move(distance, N->man, 'N'));
} else if(displacement == below){ open_list.push(robot_move(distance, N->man, 'S'));
} else if(displacement == right){ open_list.push(robot_move(distance, N->man, 'E'));
} else if(displacement == left){ open_list.push(robot_move(distance, N->man, 'W'));
}
}
}
}
robot_move current_move;
while( !open_list.empty() ) {
current_move = open_list.top();
open_list.pop();
if( cost_map[current_move.pos.x][current_move.pos.y] == FREE ){
cost_map[current_move.pos.x][current_move.pos.y] = current_move.cost;
copy_map.set(current_move.pos, current_move.direction);
}
surrounding_points = add_connected_moves( current_move, cost_map);
while( !surrounding_points.empty() ) {
open_list.push( surrounding_points.front() );
surrounding_points.pop();
}
}
char entry = 'q';
//remove all unreachables
for(auto i : N->diamonds){
float min = 1e6;
for(int x = -1; x <= 1; ++x){
for(int y = -1; y <=1; ++y){
if( (x == 0) ^ (y == 0) ){
if( (cost_map[i.x+x][i.y+y] >= 3) && (cost_map[i.x+x][i.y+y] < 1e6) ){
min = cost_map[i.x+x][i.y+y];
entry = copy_map.get(pos_t(i.x+x,i.y+y));
}
}
}
}
if(min == 1e6){
cost_map[i.x][i.y] = 1e6;
}
}
//add cost to all reachable diamonds
for(auto i : N->diamonds){
float min = 1e6;
float turn_penalty = 0;
if( cost_map[i.x][i.y] == DIAMOND){
for(int x = -1; x <= 1; ++x){
for(int y = -1; y <=1; ++y){
if( (x==0) ^ (y==0) ){
if( (cost_map[i.x+x][i.y+y] >= 3) && (cost_map[i.x+x][i.y+y] < min) && (copy_map.get(pos_t(i.x+x,i.y+y)) != DIAMOND) && ( (copy_map.get(pos_t(i.x-x,i.y-y)) > 3) || (copy_map.get(pos_t(i.x-x,i.y-y)) == FREE) ) ){
min = cost_map[i.x+x][i.y+y];
entry = copy_map.get(pos_t(i.x+x,i.y+y));
pos_t direction = pos_t(x,y); //eg. if there is a free spot on the right and the entry is facing north, the robot have to turn left
switch(entry){
case 'N':
if( direction == above){ turn_penalty = 0; }
else if(direction == right){ turn_penalty = LEFT_COST; }
else if(direction == left ){ turn_penalty = RIGHT_COST; }
break;
case 'S':
if( direction == below){ turn_penalty = 0; }
else if(direction == right){ turn_penalty = RIGHT_COST; }
else if(direction == left ){ turn_penalty = LEFT_COST; }
break;
case 'E':
if( direction == above){ turn_penalty = LEFT_COST; }
else if(direction == below){ turn_penalty = RIGHT_COST; }
else if(direction == right){ turn_penalty = 0; }
break;
case 'W':
if( direction == above){ turn_penalty = RIGHT_COST; }
else if(direction == below){ turn_penalty = LEFT_COST; }
else if(direction == left ){ turn_penalty = 0; }
default:
break;
}
if(N->man != i+direction && turn_penalty > 0){
turn_penalty += BACK_COST;
}
}
}
}
}
cost_map[i.x][i.y] = min + FORWARD_COST + turn_penalty;
}
}
for(int x = 1; x < width-1; ++x){
for(int y = 1; y < height-1; ++y){
if(cost_map[x][y] != 1e6)
copy_map.set(pos_t(x,y),cost_map[x][y]);
}
}
return move(cost_map);
}
std::queue<robot_move> Map::add_connected_moves(robot_move m, std::vector<std::vector<float>> &cost_map){
std::queue<robot_move> connected_positions;
pos_t testing_pos;
float current_cost = m.cost;
char current_direction = m.direction;
testing_pos = m.pos + above;
if (cost_map[testing_pos.x][testing_pos.y] == FREE ){
switch (current_direction) {
case 'N':
connected_positions.push(robot_move(current_cost + FORWARD_COST, testing_pos, 'N'));
break;
case 'S':
connected_positions.push(robot_move(current_cost + BACK_COST, testing_pos, 'N'));
break;
case 'E':
connected_positions.push(robot_move(current_cost + RIGHT_COST + FORWARD_COST, testing_pos, 'N'));
break;
case 'W':
connected_positions.push(robot_move(current_cost + LEFT_COST + FORWARD_COST, testing_pos, 'N'));
break;
default:
break;
}
}
testing_pos = m.pos + below;
if (cost_map[testing_pos.x][testing_pos.y] == FREE ){
switch (current_direction) {
case 'N':
connected_positions.push(robot_move(current_cost + BACK_COST, testing_pos, 'S'));
break;
case 'S':
connected_positions.push(robot_move(current_cost + FORWARD_COST, testing_pos, 'S'));
break;
case 'E':
connected_positions.push(robot_move(current_cost + LEFT_COST + FORWARD_COST, testing_pos, 'S'));
break;
case 'W':
connected_positions.push(robot_move(current_cost + RIGHT_COST + FORWARD_COST, testing_pos, 'S'));
break;
default:
break;
}
}
testing_pos = m.pos + left;
if (cost_map[testing_pos.x][testing_pos.y] == FREE ){
switch (current_direction) {
case 'N':
connected_positions.push(robot_move(current_cost + LEFT_COST + FORWARD_COST, testing_pos, 'W'));
break;
case 'S':
connected_positions.push(robot_move(current_cost + RIGHT_COST + FORWARD_COST, testing_pos, 'W'));
break;
case 'E':
connected_positions.push(robot_move(current_cost + BACK_COST, testing_pos, 'W'));
break;
case 'W':
connected_positions.push(robot_move(current_cost + FORWARD_COST, testing_pos, 'W'));
break;
default:
break;
}
}
testing_pos = m.pos + right;
if (cost_map[testing_pos.x][testing_pos.y] == FREE ){
switch (current_direction) {
case 'N':
connected_positions.push(robot_move(current_cost + LEFT_COST + FORWARD_COST, testing_pos, 'E'));
break;
case 'S':
connected_positions.push(robot_move(current_cost + RIGHT_COST + FORWARD_COST, testing_pos, 'E'));
break;
case 'E':
connected_positions.push(robot_move(current_cost + FORWARD_COST, testing_pos, 'E'));
break;
case 'W':
connected_positions.push(robot_move(current_cost + BACK_COST, testing_pos, 'E'));
break;
default:
break;
}
}
return connected_positions;
}