int main(int argc, char **argv)
{
ros::init(argc, argv, "nao_motion_control");
ros::NodeHandle n;
// Get parameters from launch file
double walk_vel, lat_vel, rot_vel;
n.getParam("sonar_status", sonar_status);
ROS_INFO("Sonar status: %.1f", sonar_status);
n.getParam("walk_vel", walk_vel);
n.getParam("lat_vel", lat_vel);
n.getParam("rot_vel", rot_vel);
forwardVel = walk_vel;
backwardVel = -forwardVel;
turnVel = rot_vel;
lateralVel = lat_vel;
// Subscribers
skelSub = n.subscribe("skeleton", 10, skeletonCallBack);
headTouchSub = n.subscribe("nao_robot/contact/tactile_touch", 1, tactileCallBack);
sonarLeftSub = n.subscribe("nao_robot/sonar/left/sonar", 1, sonarLeftCallBack);
sonarRightSub = n.subscribe("nao_robot/sonar/right/sonar", 1, sonarRightCallBack);
// Publishers
velPub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
walkPub = n.advertise<std_msgs::Int8>("walking", 10);
ttsPub = n.advertise<std_msgs::String>("text_speech", 10);
ros::Rate pubRate(10.0);
// NAO stiffness service
stiffnessDisableClient = n.serviceClient<std_srvs::Empty>("body_stiffness/disable");
stiffnessEnableClient = n.serviceClient<std_srvs::Empty>("body_stiffness/enable");
//ros::Duration time(10);
//time.sleep();
while (ros::ok)
{
if(left_hand_x != 0 && left_hand_y != 0 && right_hand_x != 0 && right_hand_y != 0){
// Erabiltzailea ohartarazteko mezuak
if(isTeleopStarted == false){
ROS_INFO("NAO teleoperation Kinect started");
isTeleopStarted = true;
}
nao_gesture_control();
velPub.publish(cmd);
}
ros::spinOnce();
pubRate.sleep();
if(exited == true){
break;
}
}
ROS_INFO("Killing nao_motion_control...");
ros::shutdown();
return 0;
}
void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg){
try {
int channel = enc::numChannels(msg->encoding);
// check if uint16 array needed
bool useData32 = false;
if (std::max(msg->width, msg->height) > 256){
useData32 = true;
NODELET_DEBUG("use data32 array");
}
_spr_img_ptr->header.stamp = msg->header.stamp;
_spr_img_ptr->header.frame_id = input_frame_from_msg;
_spr_img_ptr->width = msg->width;
_spr_img_ptr->height = msg->height;
if (!_spr_img_ptr->data16.empty()) _spr_img_ptr->data16.clear();
if (!_spr_img_ptr->data32.empty()) _spr_img_ptr->data32.clear();
// make sparse image
for (uint32_t y = 0; y < msg->height; ++y){
for (uint32_t x = 0; x < msg->width; ++x){
if(msg->data[x*channel+y*msg->step] > 125){
if (useData32) _spr_img_ptr->data32.push_back( (x << 16) | y );
else _spr_img_ptr->data16.push_back( (x << 8) | y );
}
}
}
// print number of point if enabled
if (_print_point_num) {
int size = 0;
if (useData32) size = _spr_img_ptr->data32.size();
else size = _spr_img_ptr->data16.size();
NODELET_INFO("%d point encoded. encoded area per is %f%\n", size, (100 * ((double) size)/(msg->height* msg->width)));
}
// publish sparse image message
_spr_img_pub.publish(*_spr_img_ptr);
} // end of try
catch (...) {
NODELET_ERROR("making sparse image error");
}
ros::Rate pubRate(_rate); // hz
pubRate.sleep();
} // end of do_work function