RTC::ReturnCode_t ControlEducatorVehicle::onDeactivated(RTC::UniqueId ec_id)
{
stop_robot();
return RTC::RTC_OK;
}
void estop(const std_msgs::Bool & input)
{
if(!input.data) {
stop_robot();
ROS_WARN_THROTTLE(5.0, "Stopping Robot due to EStop pressed (via msg).");
}
lastDataTime = ros::Time::now();
}
void kobuki_core(const kobuki_msgs::SensorState & sensors)
{
bool estop_pressed = (sensors.digital_input & kobuki_msgs::SensorState::DIGITAL_INPUT3)
== kobuki_msgs::SensorState::DIGITAL_INPUT3;
if(estop_pressed) {
stop_robot();
ROS_WARN_THROTTLE(5.0, "Stopping Robot due to EStop pressed.");
}
lastDataTime = ros::Time::now();
}
static void
generate_next_motion_command(void)
{
carmen_ackerman_traj_point_t waypoint;
int waypoint_index = 0;
int waypoint_status = 0;
if (!autonomous_status)
return;
waypoint = g_robot_position;
if (current_state == BEHAVIOR_SELECTOR_FOLLOWING_LANE)
waypoint_status = next_waypoint(&waypoint, &waypoint_index);
if (current_state == BEHAVIOR_SELECTOR_PARKING)
waypoint_status = next_waypoint_astar(&waypoint);
if (waypoint_status > 0) /* Goal reached? */
{
// clear_current_trajectory();
stop_robot();
return;
}
if (waypoint_status < 0) /* There is no path to the goal? */
{
// clear_current_trajectory();
stop_robot();
return;
}
if (current_state == BEHAVIOR_SELECTOR_FOLLOWING_LANE)
send_trajectory_to_robot(path.path + waypoint_index, path.path_size - waypoint_index);
if (current_state == BEHAVIOR_SELECTOR_PARKING)
publish_astar_path(path.path + waypoint_index, path.path_size - waypoint_index, g_robot_position);
//teste_stop(0.1, 2.0);
}
/**
* @brief タッチセンサがオンを検出した時に回転して回避する運動
* @param dir 方向(右がtrue、左がfalse)
*/
void ControlEducatorVehicle::rotate_move(bool dir)
{
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
if (dir)m_target_velocity_out.data.va = m_rotate_speed;
else m_target_velocity_out.data.va = -m_rotate_speed;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
double sec, usec;
usec = modf(m_rotate_time, &sec);
coil::TimeValue ts((int)sec, (int)(usec*1000000.0));
coil::sleep(ts);
stop_robot();
}
/**
* @brief タッチセンサがオンを検出した時に後退して離れる運動
*/
void ControlEducatorVehicle::back_move()
{
m_target_velocity_out.data.vx = -m_back_speed;
m_target_velocity_out.data.vy = 0;
m_target_velocity_out.data.va = 0;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
double sec, usec;
usec = modf(m_back_time, &sec);
coil::TimeValue ts((int)sec, (int)(usec*1000000.0));
coil::sleep(ts);
stop_robot();
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "estop_safety_controller");
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
bool auto_estop = true;
nhPriv.param("auto_estop", auto_estop, auto_estop);
bool use_kobuki_din3_estop = false;
nhPriv.param("use_kobuki_din3_estop", use_kobuki_din3_estop, use_kobuki_din3_estop);
ros::Subscriber sub = nh.subscribe ("estop", 1, estop);
ros::Subscriber subKobuki;
if(use_kobuki_din3_estop)
subKobuki = nh.subscribe("mobile_base/sensors/core", 1, kobuki_core);
pub = nh.advertise<geometry_msgs::Twist>("cmd_vel_mux/input/estop", 1);
ROS_INFO("estop_safety_controller: auto_estop: %s use_kobuki_din3_estop: %s",
auto_estop ? "enabled" : "disabled", use_kobuki_din3_estop ? "enabled" : "disabled");
ros::Rate r(10);
while (ros::ok()) {
ros::Duration timeSinceLastData = ros::Time::now() - lastDataTime;
if(timeSinceLastData > ros::Duration(1.0)) {
if(auto_estop) {
ROS_WARN_THROTTLE(1.0, "No EStop data available - Stopping robot!");
stop_robot();
} else {
ROS_WARN_THROTTLE(5.0, "No EStop data available!");
}
}
ros::spinOnce();
r.sleep();
}
}
void
motion_planning_obstacle_avoiding_handler()
{
carmen_ackerman_motion_command_p next_motion_commands_vector;
int next_motion_command_vector_index, motion_commands_trajectory_size;
double time_budget;
carmen_ackerman_traj_point_t current_robot_position;
int hit_obstacle, i;
int trajectory_lenght;
double current_max_v;
carmen_ackerman_traj_point_t *trajectory;
int trajectory_length;
carmen_ackerman_traj_point_t robot_position;
static int already_planning = 0;
if (current_algorithm == CARMEN_BEHAVIOR_SELECTOR_RRT || current_state == BEHAVIOR_SELECTOR_PARKING)
return;
if (already_planning)
return;
already_planning = 1;
if (g_current_trajectory == NULL)
{
already_planning = 0;
return;
}
trajectory = g_current_trajectory;
trajectory_length = g_current_trajectory_length;
robot_position = g_robot_position;
if (trajectory_length <= 0) // invalid trajectory // @@@ Alberto: isso acontece? Se acontence, por que acontece?
{
printf("trajectory_length <= 0 in motion_planning_obstacle_avoiding_handler()\n");
already_planning = 0;
return;
}
next_motion_command_vector_index = (current_motion_command_vetor_index + 1) % NUM_MOTION_COMMANDS_VECTORS;
next_motion_commands_vector = motion_commands_vector[next_motion_command_vector_index];
current_robot_position.x = robot_position.x;
current_robot_position.y = robot_position.y;
current_robot_position.theta = robot_position.theta;
current_robot_position.v = robot_position.v;
current_robot_position.phi = robot_position.phi;
time_budget = 20.0; // @@@ tem que ver como melhor determinar este valor
current_max_v = carmen_robot_ackerman_config.max_v;
do
{
motion_commands_trajectory_size = motion_planner_compute_trajectory(next_motion_commands_vector, trajectory, trajectory_length, current_robot_position,
NUM_MOTION_COMMANDS_PER_VECTOR, time_budget, current_max_v);
hit_obstacle = 0;
trajectory_lenght = build_predicted_trajectory(next_motion_commands_vector, motion_commands_trajectory_size, current_robot_position);
for (i = 0; i < trajectory_lenght; i++)
{
if (pose_hit_obstacle(to_carmen_point_t(&(trajectory_vector_of_points[i])), map_vector[current_map], &carmen_robot_ackerman_config))
{
current_max_v *= 0.7;
hit_obstacle = 1;
break;
}
}
} while (hit_obstacle && fabs((current_max_v /*- (current_robot_position.v / 10.0)*/)) > 0.05); // @@@ Alberto: Isso nao trata velocidades negativas...
if ((motion_commands_trajectory_size == 0) || (autonomous_status == 0) || (hit_obstacle == 1))
{
stop_robot();
already_planning = 0;
// printf("hit = %d\n", count++);
return;
}
nun_motion_commands[next_motion_command_vector_index] = motion_commands_trajectory_size;
current_motion_command_vetor_index = next_motion_command_vector_index;
carmen_obstacle_avoider_publish_base_ackerman_motion_command(motion_commands_vector[current_motion_command_vetor_index], nun_motion_commands[current_motion_command_vetor_index]);
IPC_listen(50);
publish_obstacle_avoider_path(motion_commands_vector[current_motion_command_vetor_index], nun_motion_commands[current_motion_command_vetor_index]);
IPC_listen(50);
publish_motion_planner_path(motion_commands_vector[current_motion_command_vetor_index], nun_motion_commands[current_motion_command_vetor_index]);
IPC_listen(50);
already_planning = 0;
}
//implemetation of functions
int start_finding(int start_x, int start_y)
{
int inter = 0;
int token = 0;
int cur_x = start_x, cur_y = start_y;
int dir = SOUTH;
int ppath = -1;
int npop = 0; //how many points should be poped
struct POINT *cur_p = NULL;
struct POINT *tmp_p = NULL;
int ret = 0;
#ifdef DEBUG
inter = Robot_GetIntersections();
#else
inter = get_intersection();
#endif
cur_p = mark_point(cur_x, cur_y, inter);
dir = get_direction(cur_p);
#ifdef DEBUG
printf("start point: ");
print_point(cur_p);
printf("\n");
#endif
while(token < TOKEN_COUNT)
{
#ifdef DEBUG
//inter = Robot_GetIntersections();
//print_intersection(inter);
#endif
if(points[cur_x][cur_y].detected == 0)
cur_p = mark_point(cur_x, cur_y, inter);
else
cur_p = &points[cur_x][cur_y];
push(cur_p);
//print_stack();
if(dir = get_direction(cur_p))
{
//update current point
switch(dir)
{
case EAST:
cur_x += 1;
break;
case SOUTH:
cur_y -= 1;
break;
case WEST:
cur_x -= 1;
break;
case NORTH:
cur_y += 1;
break;
}
#ifdef DEBUG
print_direction(cur_p, dir);
ret = aud_move(cur_p, dir);
#else
//move one step
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(cur_x, 2);
display_goto_xy(10, 0);
display_int(cur_y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
ret = move(cur_x, cur_y);
#endif
#ifdef DEBUG
inter = Robot_GetIntersections();
#else
inter = get_intersection();
#endif
cur_p = mark_point(cur_x, cur_y, inter);
#ifdef DEBUG
print_point(cur_p);
#endif
if(ret == ROBOT_SUCCESS)
{
#ifndef DEBUG
#endif
}
else if(ret == ROBOT_TOKENFOUND)
{
tmp_p = &points[cur_x][cur_y];
if(tmp_p->has_token == 0)
{
tmp_p->has_token = 1;
token++;
#ifdef DEBUG
printf("[%d. token]\n", token);
#endif
}
else
{
#ifdef DEBUG
printf("[not a new token]\n");
#endif
}
if(token == TOKEN_COUNT)
{
//all token were found, go back to start point
#ifdef DEBUG
printf("going back to start point......\n");
#endif
push(cur_p);
ppath = find_shortest_path(cur_p->x, cur_p->y, START_X, START_Y);
if(ppath)
{
//going back to last open point
ppath--;
while(ppath >= 0)
{
tmp_p = shortest_path[ppath];
dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
#ifdef DEBUG
print_point(tmp_p);
printf("\n");
ROBOT_MOVE(tmp_p->x, tmp_p->y);
#else
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(tmp_p->x, 2);
display_goto_xy(10, 0);
display_int(tmp_p->y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
move(tmp_p->x, tmp_p->y);
#endif
cur_p = tmp_p;
ppath--;
}
//delete the path in stack
pop(npop + 1);
cur_p = tmp_p;
cur_x = cur_p->x;
cur_y = cur_p->y;
}
#ifdef DEBUG
printf("task finished!\n");
#else
beep();
#endif
break;
}
}
else
{
#ifdef DEBUG
printf("move failed!\n");
#endif
}
}
else
{
//there is no ways forward, go back to nearest open point
tmp_p = get_last_open_point();
npop = stack_pointer - get_stack_index(tmp_p->x, tmp_p->y);
#ifdef DEBUG
printf("going back to (%d, %d)\n", tmp_p->x, tmp_p->y);
#endif
if(tmp_p)
{
if((tmp_p->x == START_X) && (tmp_p->y == START_Y) && !IS_OPEN_POINT(points[tmp_p->x][tmp_p->y]))
{
#ifdef DEBUG
return 0;
#else
stop_robot();
beep();
return 0;
#endif
}
ppath = find_shortest_path(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
if(ppath)
{
//going back to last open point
ppath--;
while(ppath >= 0)
{
tmp_p = shortest_path[ppath];
dir = calc_direction(cur_p->x, cur_p->y, tmp_p->x, tmp_p->y);
#ifdef DEBUG
ROBOT_MOVE(tmp_p->x, tmp_p->y);
#else
display_clear(0);
display_goto_xy(0, 0);
display_int(cur_p->x, 2);
display_goto_xy(3, 0);
display_int(cur_p->y, 2);
display_goto_xy(7, 0);
display_int(tmp_p->x, 2);
display_goto_xy(10, 0);
display_int(tmp_p->y, 2);
display_goto_xy(0, 1);
display_int(g_dir, 3);
display_goto_xy(5, 1);
display_int(dir, 3);
display_goto_xy(0, 2);
display_int(cur_p->inter&0xF0, 3);
display_update();
move(tmp_p->x, tmp_p->y);
#endif
cur_p = tmp_p;
ppath--;
}
//delete the path in stack
pop(npop + 1);
cur_p = tmp_p;
cur_x = cur_p->x;
cur_y = cur_p->y;
}
else
{
//was already at every point and back to start point
//task should be ended
//that means, not enough token can be found
#ifdef DEBUG
printf("task ended without enough token were found.\n");
#endif
break;
}
}
}
#ifdef DEBUG
printf("\n");
#endif
}
return 0;
}
RTC::ReturnCode_t ControlEducatorVehicle::onExecute(RTC::UniqueId ec_id)
{
if (m_target_velocity_inIn.isNew())
{
m_target_velocity_inIn.read();
vx = m_target_velocity_in.data.vx;
vy = m_target_velocity_in.data.vy;
va = m_target_velocity_in.data.va;
}
if (m_touchIn.isNew())
{
while(m_touchIn.isNew())m_touchIn.read();
if (m_touch.data.length() >= 2)
{
touch_r = m_touch.data[0];
touch_l = m_touch.data[1];
if (vx > 0)
{
if (m_touch.data[0])
{
stop_robot();
back_move();
rotate_move(true);
}
else if (m_touch.data[1])
{
stop_robot();
back_move();
rotate_move(false);
}
}
}
}
if (m_ultrasonicIn.isNew())
{
while(m_ultrasonicIn.isNew())m_ultrasonicIn.read();
if (m_ultrasonic.ranges.length() >= 1)
{
range = m_ultrasonic.ranges[0];
if (vx > 0)
{
if (m_ultrasonic.ranges[0] > m_sensor_height)
{
stop_flag = true;
stop_robot();
m_angle.data = 0;
setTimestamp(m_angle);
m_angleOut.write();
double a = 0;
bool ret = search_ground(a);
m_angle.data = 0;
setTimestamp(m_angle);
m_angleOut.write();
if (ret)
{
turn_move(a);
}
coil::TimeValue ts(2, 0);
coil::sleep(ts);
}
else
{
stop_flag = false;
}
}
}
}
if(m_light_reflectIn.isNew())
{
while(m_light_reflectIn.isNew())m_light_reflectIn.read();
if(m_light_reflect.data < 1)
{
stop_flag = true;
}
else
{
stop_flag = false;
}
}
if (vx <= 0)
{
stop_flag = false;
}
if (stop_flag)
{
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
m_target_velocity_out.data.va = 0;
}
else
{
m_target_velocity_out.data.vx = vx;
m_target_velocity_out.data.vy = vy;
m_target_velocity_out.data.va = va;
}
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
return RTC::RTC_OK;
}
/**
* @brief 指定した角度だけ回転させる
* @param a 角度
*/
void ControlEducatorVehicle::turn_move(double a)
{
int max_count = 100;
coil::TimeValue ts(0,10000);
double spos = 0;
for (int i = 0; i < max_count; i++)
{
if (m_current_poseIn.isNew())
{
m_current_poseIn.read();
spos = m_current_pose.data.heading;
break;
}
coil::sleep(ts);
if (i == max_count - 1)return;
}
max_count = 1000;
double data_new_count = 0;
double max_data_new_count = 100;
for (int i = 0; i < max_count; i++)
{
if (m_current_poseIn.isNew())
{
while(m_current_poseIn.isNew())m_current_poseIn.read();
double pos = m_current_pose.data.heading - spos;
double k = 1;
m_target_velocity_out.data.vx = 0;
m_target_velocity_out.data.vy = 0;
/*if (a > 0)m_target_velocity_out.data.va = m_rotate_speed;
else m_target_velocity_out.data.va = -m_rotate_speed;*/
double diff = (a - pos);
double vela = k * diff;
if(vela > m_rotate_speed)vela = m_rotate_speed;
if(vela < -m_rotate_speed)vela = -m_rotate_speed;
m_target_velocity_out.data.va = vela;
setTimestamp(m_target_velocity_out);
m_target_velocity_outOut.write();
if(sqrt(pow(diff,2)) < 0.03)
{
stop_robot();
return;
}
/*if (a > 0)
{
if (pos > a)
{
stop_robot();
return;
}
}
else
{
if (pos < a)
{
stop_robot();
return;
}
}*/
data_new_count = 0;
}
else
{
data_new_count += 1;
if(data_new_count > max_data_new_count)
{
stop_robot();
return;
}
coil::sleep(ts);
}
}
stop_robot();
}
//main
int main(int argc, char **argv)
{
ros::init(argc, argv, "Control");
ros::NodeHandle n;
std::string nav_topic;
bool stopped = true;
mode_ = standby;
//reset kinect angle
kinect_tilt_ang_ = 0;
kinect_tilt_vel_ = 0;
kinect_tilt_pre_ = 50;
kinect_last_publish = ros::Time::now();
joint_state_seq = 0;
//initalize toggles
for (int i = 0; i < 30 ; i++)
{
wii_togg[i] = false;
}
wii_timeout = ros::Time::now();
classic_timeout = ros::Time::now();
//setup dynamic reconfigure gui
dynamic_reconfigure::Server<mesh_bot_control::mesh_bot_control_ParamsConfig> srv;
dynamic_reconfigure::Server<mesh_bot_control::mesh_bot_control_ParamsConfig>::CallbackType f;
f = boost::bind(&setparamsCallback, _1, _2);
srv.setCallback(f);
//get topic name
nav_topic = n.resolveName("nav_twist");
//check to see if user has defined an image to subscribe to
if (nav_topic == "nav_twist")
{
ROS_WARN("Control: navigation twist has not beeen remaped! Typical command-line usage:\n"
"\t$ ./Contestop_pubrol twist:=<twist topic> [transport]");
}
// create subscriptions
nav_sub = n.subscribe( nav_topic ,100, navigation_callback);
wiimote_sub = n.subscribe("wiimote/state" ,100,wiimote_callback);
classic_sub = n.subscribe("wiimote/classic" ,100,classic_callback);
//create publications
wiimote_led_pub = n.advertise<wiimote::LEDControl>("wiimote/leds" ,100);
wiimote_rum_pub = n.advertise<wiimote::RumbleControl>("wiimote/rumble" ,100);
motor_pub = n.advertise<geometry_msgs::Twist>("con_vel" ,100);
sound_pub = n.advertise<sound_play::SoundRequest>("robotsound" ,100);
kinect_led_pub = n.advertise<std_msgs::UInt16>("/led_option" ,100);
kinect_tilt_pub = n.advertise<std_msgs::Float64>("/tilt_angle" ,1);
joint_state_pub = n.advertise<sensor_msgs::JointState>("/kinect_state" ,100);
//set rate to 10 hz
ros::Rate loop_rate(10);
//run main loop
while (ros::ok())
{
//check calbacks
ros::spinOnce();
if(robot_init)
{
ros::Duration(7).sleep();
say("Hello World. My name is mesh bot. Please press the one and two buttons on the wiimote to connect");
robot_init = false;
}
if(mode_==standby)
{
if(!stopped)
{
stop_robot();
stopped = true;
}
}
else if(mode_ == remote_wiimote)
{
motor_pub.publish(wii_twist);
stopped = false;
//set tilt velocity in deg/sec
kinect_tilt_vel_ = -wii_twist.angular.y * (180/3.14);
if(wii_timeout < ros::Time::now())
{
change_mode(standby);
}
}
else if(mode_ == autonomous_mapping)
{
motor_pub.publish(nav_twist);
stopped = false;
}
//publish the tilt message
kinect_tilt();
loop_rate.sleep();
}
return 0;
}
//calback for the wimote state
void wiimote_callback(const wiimote::State::ConstPtr& state)
{
wii_timeout = ros::Time::now() + ros::Duration(TIMEOUT_TIME);
//sets inital conection and resets if node restarted
//when wiimote is killed and restarted it continuously
// publishes its last message the checking of home handels this
if(wiimote_init && (!state->buttons[MSG_BTN_HOME] && wii_dis))
{
ROS_INFO("Control: Wiimote Conected");
wiimote::RumbleControl rumble;
//makes wiimote rumble 3 times
rumble.rumble.switch_mode = rumble.rumble.REPEAT;
rumble.rumble.num_cycles = 3;
rumble.rumble.set_pulse_pattern_size(2);
rumble.rumble.pulse_pattern[0] = .25;
rumble.rumble.pulse_pattern[1] = .5;
ros::Duration(3).sleep();
wiimote_rum_pub.publish(rumble);
say("wiimote connected. Mesh Bot standing by");
change_mode(standby);
wii_dis = false;
wiimote_init = false;
}
//plus an minus sounds plus and minus ar mixed up with
//a and b in the the apis map
//plus
if(check_togg(state->buttons[MSG_BTN_PLUS], MSG_BTN_PLUS))
{
say(params.plus_mesage);
}
//minus
if(check_togg(state->buttons[MSG_BTN_MINUS], MSG_BTN_MINUS))
{
play(params.minus_sound);
}
//shut down code on
if(state->buttons[MSG_BTN_HOME] && !wii_dis)
{
//must be held for 3 seconds
if(!wii_togg[MSG_BTN_HOME])
{
home_press_begin = ros::Time::now();
wii_togg[MSG_BTN_HOME] = true;
}
if(wii_togg[MSG_BTN_HOME] && state->buttons[MSG_BTN_HOME] && (ros::Time::now() - home_press_begin) > ros::Duration(3))
{
stop_robot();
system("rosnode kill wiimote");
wii_togg[MSG_BTN_HOME] = false;
wii_dis = true;
wiimote_init = true;
say("wiimote disconnected.. Please press the one and two buttons to reconnect");
change_mode(standby);
}
}
//behavior in standby
if(mode_ == standby)
{
//1 button
if(check_togg(state->buttons[MSG_BTN_1], MSG_BTN_1))
{
change_mode(remote_wiimote);
}
//two button
if(check_togg(state->buttons[MSG_BTN_2], MSG_BTN_2))
{
change_mode(autonomous_mapping);
}
}
//wiimote behavior in wiimote mode
if(mode_ == remote_wiimote)
{
//initalize twist
if(!classic_connected)
{
wii_twist.angular.x = 0;
wii_twist.angular.y = 0;
wii_twist.angular.z = 0;
wii_twist.linear.x = 0;
wii_twist.linear.y = 0;
wii_twist.linear.z = 0;
}
//button 1 moves to standby
if(check_togg(state->buttons[MSG_BTN_1], MSG_BTN_1))
{
change_mode(standby);
}
//button 2 moves to autonomous
if(check_togg(state->buttons[MSG_BTN_2], MSG_BTN_2))
{
change_mode(autonomous_mapping);
}
//setup local variables
bool nunchuk_conected = false;
double turbo_linear = 1;
double turbo_angular = 1;
if( state->nunchuk_joystick_raw[0] != 0 ||
state->nunchuk_joystick_raw[1] != 0 )
{
nunchuk_conected = true;
}
//nunchuck and wiimote
if(nunchuk_conected)
{
//nunchuk boost handlers
if(state->nunchuk_buttons[MSG_BTN_Z])
{
turbo_angular += params.turbo_angular;
turbo_linear += params.turbo_linear;
}
//compute controlls
if(fabs(state->nunchuk_joystick_zeroed[1]) >= 0.1)
{
wii_twist.linear.x = state->nunchuk_joystick_zeroed[1] * params.base_linear_speed * turbo_linear;
}
else
{
wii_twist.linear.x = 0;
}
if(fabs(state->nunchuk_joystick_zeroed[0]) >= 0.1)
{
wii_twist.linear.y = state->nunchuk_joystick_zeroed[0] * params.base_linear_speed * turbo_linear;
}
else
{
wii_twist.linear.y = 0;
}
//controll for the camera
//down is positive in twist
//the zeroed one jumped to 1000 because the wimote library is broken
if(state->nunchuk_buttons[MSG_BTN_C])
{
wii_twist.angular.z = -(state->nunchuk_acceleration_raw.x - 127)/50 * params.base_rot_speed * turbo_angular;
wii_twist.angular.y = (state->nunchuk_acceleration_raw.y - 127)/50 * params.base_rot_speed * turbo_angular;
}
else if(state->buttons[MSG_BTN_B])
{
wii_twist.angular.z = -(state->linear_acceleration_zeroed.x)/10 * params.base_rot_speed * turbo_angular;
wii_twist.angular.y = (state->linear_acceleration_zeroed.y)/10 * params.base_rot_speed * turbo_angular;
}
else
{
//buttons are named with the wiimote on the side
if(state->buttons[MSG_BTN_LEFT])
{
wii_twist.angular.y = -params.base_rot_speed * turbo_linear * params.d_pad_percent/100;
}
else if(state->buttons[MSG_BTN_RIGHT])
{
wii_twist.angular.y = params.base_rot_speed * turbo_linear * params.d_pad_percent/100;
}
if(state->buttons[MSG_BTN_UP])
{
wii_twist.angular.z = params.base_rot_speed * turbo_angular * params.d_pad_percent/100;
}
else if(state->buttons[MSG_BTN_DOWN])
{
wii_twist.angular.z = -params.base_rot_speed * turbo_angular * params.d_pad_percent/100;
}
}
}
//just the wiimote
else if (classic_connected == false)
{
//there constants for the buttons are wrong
//a button boost
if(state->buttons[MSG_BTN_A])
{
turbo_angular += params.turbo_angular;
turbo_linear += params.turbo_linear;
}
//camera control b and tilt
if(state->buttons[MSG_BTN_B])
{
wii_twist.angular.z = -(state->linear_acceleration_zeroed.x)/10 * params.base_rot_speed * turbo_angular;
wii_twist.angular.y = (state->linear_acceleration_zeroed.y)/10 * params.base_rot_speed * turbo_angular;
}
//dpad handlers for movement
//buttons are named with the wiimote on the side
if(state->buttons[MSG_BTN_LEFT])
{
wii_twist.linear.x = params.base_linear_speed * turbo_linear * params.d_pad_percent/100;
}
else if(state->buttons[MSG_BTN_RIGHT])
{
wii_twist.linear.x = -params.base_linear_speed * turbo_linear * params.d_pad_percent/100;
}
if(state->buttons[MSG_BTN_UP])
{
wii_twist.linear.y = params.base_linear_speed * turbo_angular * params.d_pad_percent/100;
}
else if(state->buttons[MSG_BTN_DOWN])
{
wii_twist.linear.y = -params.base_linear_speed * turbo_angular * params.d_pad_percent/100;
}
}
else
{
if(classic_timeout < ros::Time::now())
{
//havent received a message from classic in a while
classic_connected = false;
}
//classic controller
}
}
//wiimote behavior in autonomous mode
if(mode_==autonomous_mapping)
{
//button 2 moves back to standby
if(check_togg(state->buttons[MSG_BTN_2], MSG_BTN_2))
{
change_mode(standby);
}
//button 1 moves to wiimote mode
if(check_togg(state->buttons[MSG_BTN_1], MSG_BTN_1))
{
change_mode(remote_wiimote);
}
//@TODO controll of autonomous modes
}
}