bool checkCopter(double copter_x, double copter_y, double copter_z,
double x, double y, double z, char &colour){
// Initially red. For when the QC is far away.
colour = 'r';
// Region when marker should turn yellow. When the QC is reaching the tapping vicinity.
double markerZone_height = z + (Radius/2) + 0.0625;
double markerZone_bottom_x = x - Radius*2;
double markerZone_top_x = x + Radius*2;
double markerZone_bottom_y = y - Radius*2;
double markerZone_top_y = y + Radius*2;
//This uses one tenth of the radius. If it seems to be too much, raise the "10" below.
//The radius does need to be present otherwise it may not detect the copter every time.
double roomba_height = z + (Radius/2) + 0.0625;
//The x coordinates that the copter has to be in
double bottom_x = x - Radius*2;
double top_x = x + Radius*2;
//The y coordinates that the copter has to be in
double bottom_y = y - Radius*2;
double top_y = y + Radius*2;
//If the copter coordinates are (0,0,0), something is probably wrong
if (copter_x == 0 && copter_y == 0 && copter_z == 0) return false;
// If the copter is close to the marker.
if(markerZone_bottom_x <= copter_x && copter_x <= markerZone_top_x){
if(markerZone_bottom_y <= copter_y && copter_y <= markerZone_top_y){
if(copter_z <= markerZone_height){
colour = 'y';
}
}
}
//If the copter is within the radius, return true
if (bottom_x <= copter_x && copter_x <= top_x){
if (bottom_y <= copter_y && copter_y <= top_y){
if((copter_z - 0.182) <= roomba_height){ // 0.182 is the length between copter centre of mass and the tip of its legs (base)
timer_5.stop();
timer_20.stop();
timer_5.start();
timer_20.start();
count_5 = looprate*3; // 3 so it never goes lower than limit
count_20 = looprate*3;
colour = 'g';
return true;
}
}
}
return false;
}
void connection_cb(bool connected) {
// if connection changes, start delayed version request
version_retries = RETRIES_COUNT;
if (connected)
autopilot_version_timer.start();
else
autopilot_version_timer.stop();
// add/remove APM diag tasks
if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
"Extra diagnostic disabled.");
#else
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
}
else {
UAS_DIAG(uas).removeByName(mem_diag.getName());
UAS_DIAG(uas).removeByName(hwst_diag.getName());
ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
}
}
void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (!uas->is_my_target(sysid)) {
ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
return;
}
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
// update context && setup connection timeout
uas->update_heartbeat(hb.type, hb.autopilot);
uas->update_connection_status(true);
timeout_timer.stop();
timeout_timer.start();
// build state message after updating uas
auto state_msg = boost::make_shared<mavros::State>();
state_msg->header.stamp = ros::Time::now();
state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);
state_pub.publish(state_msg);
hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
}
FollowerFSM() : hold(true),
currentState(STOPPED),
nextState(STOPPED),
nPriv("~")
{
//hold = true;
//currentState = STOPPED;
//nextState = STOPPED;
nPriv.param<std::string>("world_frame", world_frame, "world_lol");
nPriv.param<std::string>("robot_frame", robot_frame, "robot");
nPriv.param<std::string>("fixed_frame", fixed_frame_id, "odom");
nPriv.param<double>("minimum_distance", minimum_distance, 2);
nPriv.param<double>("planner_activation_distance", planner_activation_distance, 1.7);
nPriv.param("minimum_planner_distance", minimum_planner_distance, 1.7);
nPriv.param("distance_to_target", distance_to_target, 1.5);
nPriv.param<double>("kv", kv, 0.03);
nPriv.param<double>("kalfa", kalfa, 0.08);
//nPriv.param<double>("rate", frequency, 10);
rate = new ros::Rate(10.0);
nPriv.param<std::string>("control_type", cType, "planner");
if(cType == "planner")
{ ac = new MoveBaseClient("move_base", true);}
notReceivedNumber = 0;
sub = n.subscribe("person_position", 1, &FollowerFSM::receiveInformation, this);
cmdPub = n.advertise<geometry_msgs::Twist>("twist_topic", 1);
timer = n.createTimer(ros::Duration(0.1), &FollowerFSM::checkInfo, this); //If we didn't receive any position on 2 seconds STOP the robot!
timer.start();
}
/**
* Callback
* initialisation de la liste des publishers inactif pendant une periode T
*/
void init_caplist(const ros::TimerEvent& ){
ROS_INFO("Initialisation des Publishers.......!");
timer2.stop();
for(int i=0; i<(int)CAP_LIST.size()+1; i++){
chatter_pub[i].shutdown();
}
//ros::spinOnce();
timer2.start();
ROS_INFO("Initialisation terminee *");
}
void setVelocity(double linearVel, double angularVel)
{
// Stopping and starting the timer causes it to start counting from 0 again.
// As long as this is called before the kill swith timer reaches killSwitchTimeout seconds
// the rover's kill switch wont be called.
killSwitchTimer.stop();
killSwitchTimer.start();
velocity.linear.x = linearVel * 1.5;
velocity.angular.z = angularVel * 8; //scaling factor for sim; removed by aBridge node
velocityPublish.publish(velocity);
}
void RosAriaNode::sonarConnectCb()
{
if (!robot->tryLock()) {
ROS_ERROR("Skipping sonarConnectCb because could not lock");
return;
}
if (!robot->areSonarsEnabled())
{
robot->enableSonar();
sonar_tf_timer.start();
}
robot->unlock();
}
void openCamera(ros::Timer &timer)
{
timer.stop();
while (!camera_)
{
try
{
boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
camera_ = boost::make_shared<PMDCamboardNano>(device_serial, plugin_dir, source_plugin, process_plugin);
device_serial = camera_->getSerialNumber().c_str();
updater.setHardwareIDf("%s", device_serial.c_str());
NODELET_INFO("Opened PMD camera with serial number \"%s\"", camera_->getSerialNumber().c_str());
loadCalibrationData();
NODELET_INFO("Loaded calibration data");
camera_->update();
camera_info_ = camera_->getCameraInfo();
}
catch (PMDCameraNotOpenedException& e)
{
camera_state_ = CAMERA_NOT_FOUND;
if (device_serial != "")
{
std::stringstream err;
err << "Unable to open PMD camera with serial number " << device_serial;
state_info_ = err.str();
NODELET_INFO("%s",state_info_.c_str());
}
else
{
state_info_ = "Unable to open PMD camera..";
NODELET_INFO("%s",state_info_.c_str());
}
boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
camera_.reset();
}
updater.update();
boost::this_thread::sleep(boost::posix_time::seconds(open_camera_retry_period));
}
timer.start();
}
/********** callback for the cmd velocity from the autonomy **********/
void cmd_vel_callback(const geometry_msgs::Twist& msg)
{
watchdogTimer.stop();
error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
error_yaw = msg.angular.z - body_vel.angular.z;
//std::cout << "error yaw: " << error_yaw << std::endl;
// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
{
errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
}
last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
last_error = error;
last_error_yaw = error_yaw;
error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
error_pub.publish(error_gm);
errorDot_pub.publish(errorDot_gm);
if (start_autonomous)
{
recieved_command_from_tracking = true;
}
watchdogTimer.start();
}
// Callback for estimator
void featureCB(const aruco_ros::CenterConstPtr& center)
{
// Disregard erroneous tag tracks
if (markerID.compare(center->header.frame_id) != 0)
{
return;
}
// Switching
if (artificialSwitching)
{
if (!estimatorOn)
{
return;
}
}
else
{
// Feature in FOV
watchdogTimer.stop();
estimatorOn = true;
}
// Time
ros::Time timeStamp = center->header.stamp;
double timeNow = timeStamp.toSec();
double delT = timeNow - lastImageTime;
lastImageTime = timeNow;
// Object trans w.r.t. image frame, for ground truth
Vector3d trans;
tf::StampedTransform transform;
tfl.waitForTransform("image","ugv0",timeStamp,ros::Duration(0.1));
tfl.lookupTransform("image","ugv0",timeStamp,transform);
tf::Vector3 temp_trans = transform.getOrigin();
trans << temp_trans.getX(),temp_trans.getY(),temp_trans.getZ();
// Object pose w.r.t. image frame
if (deadReckoning)
{
tfl.waitForTransform("image",string("marker")+markerID,timeStamp,ros::Duration(0.1));
tfl.lookupTransform("image",string("marker")+markerID,timeStamp,transform);
try
{
// Additional transforms for predictor
tf::StampedTransform tfWorld2Marker;
tf::StampedTransform tfMarker2Odom;
tfl.waitForTransform("world",string("marker")+markerID,timeStamp,ros::Duration(0.1));
tfl.lookupTransform("world",string("marker")+markerID,timeStamp,tfWorld2Marker);
tfl.waitForTransform("ugv0/base_footprint","ugv0/odom",timeStamp,ros::Duration(0.1));
tfl.lookupTransform("ugv0/base_footprint","ugv0/odom",timeStamp,tfMarker2Odom);
// Save transform
tf::Quaternion temp_quat = tfWorld2Marker.getRotation();
Quaterniond qW2M = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
temp_quat = tfMarker2Odom.getRotation();
Quaterniond qM2O = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
qWorld2Odom = qW2M*qM2O;
}
catch (tf::TransformException e)
{
}
}
// else, use quaternion from image to ugv0 transform
tf::Quaternion temp_quat = transform.getRotation();
Quaterniond quat = Quaterniond(temp_quat.getW(),temp_quat.getX(),temp_quat.getY(),temp_quat.getZ());
// Undistort image coordinates. Returns normalized Euclidean coordinates
double ptsArray[2] = {center->x,center->y};
cv::Mat pts(1,1,CV_64FC2,ptsArray);
cv::Mat undistPts;
cv::undistortPoints(pts,undistPts,camMat,distCoeffs);
Vector3d x;
x << undistPts.at<double>(0,0),undistPts.at<double>(0,1),1/trans(2);
// Target velocities expressed in camera coordinates
Vector3d vTc = quat*vTt;
// Observer velocities
Vector3d b = vTc - vCc;
Vector3d w = -wGCc; //Vector3d::Zero();
// EKF update
Matrix<double,3,2> K = P*H.transpose()*(H*P*H.transpose()+R).inverse();
xhat += K*(x.head<2>()-xhat.head<2>());
P = (Matrix3d::Identity()-K*H)*P;
// Publish output
publishOutput(x,xhat,trans,timeStamp);
if (!artificialSwitching)
{
// Restart watchdog timer for feature visibility check
watchdogTimer.start();
}
}
void CurrentActionListCallback(const supervisor_msgs::ActionsList::ConstPtr& msg)
{
try
{
if(!msg->actions.empty())
{
for(int i = 0 ; i < msg->actions.size() ; ++i)
{
for(int j = 0 ; j < msg->actions[i].actors.size() ; ++j)
{
if(msg->actions[i].actors[j] == "HERAKLES_HUMAN1")
{
ROS_INFO("[robot_observer] Action detected");
if(msg->actions[i].focusTarget=="RED_CUBE"){
task_started_=true;
current_action_position_=red_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="BLACK_CUBE"){
task_started_=true;
current_action_position_=black_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="BLUE_CUBE"){
task_started_=true;
current_action_position_=blue_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="GREEN_CUBE2"){
task_started_=true;
current_action_position_=green_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="PLACEMAT_RED"){
task_started_=true;
current_action_position_=placemat_position_;
current_action_ =msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
}
}
}
}
}
catch (HeadManagerException& e )
{
ROS_ERROR("[robot_observer] Exception was caught : %s",e.description().c_str());
}
}
bool grab_vicon_pose_callback(vicon_mocap::viconGrabPose::Request& req, vicon_mocap::viconGrabPose::Response& resp)
{
ROS_INFO("Got request for a VICON pose");
updateTimer.stop();
tf::StampedTransform transform;
//tf::Quaternion quat;
if (not segment_data_enabled)
{
MyClient.EnableSegmentData();
ROS_ASSERT(MyClient.IsSegmentDataEnabled().Enabled);
segment_data_enabled = true;
}
// Gather data:
int N = req.n_measurements;
double accum_trans[3] = {0, 0, 0};
double accum_quat[4] = {0, 0, 0, 0};
Result::Enum the_result;
int n_success = 0;
for (int k = 0; k < N; k++)
{
ros::Duration(1 / vicon_capture_rate).sleep(); // Wait at least as long as vicon needs to capture the next frame
if ((the_result = MyClient.GetFrame().Result) == Result::Success)
{
try
{
Output_GetSegmentGlobalTranslation trans = MyClient.GetSegmentGlobalTranslation(req.subject_name,
req.segment_name);
Output_GetSegmentGlobalRotationQuaternion quat =
MyClient.GetSegmentGlobalRotationQuaternion(req.subject_name, req.segment_name);
if ((!trans.Occluded) && (!quat.Occluded))
{
accum_trans[0] += trans.Translation[0] / 1000;
accum_trans[1] += trans.Translation[1] / 1000;
accum_trans[2] += trans.Translation[2] / 1000;
accum_quat[0] += quat.Rotation[3];
accum_quat[1] += quat.Rotation[0];
accum_quat[2] += quat.Rotation[1];
accum_quat[3] += quat.Rotation[2];
n_success++;
}
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s", ex.what());
resp.success = false;
return false; // TODO: should we really bail here, or just try again?
}
}
else
{
if (the_result != Result::NoFrame)
{
ROS_ERROR_STREAM("GetFrame() returned " << (int)the_result);
}
}
}
ROS_INFO("Averaging %d measurements", n_success);
// Average the data:
double normalized_quat[4];
double quat_norm = sqrt(
accum_quat[0] * accum_quat[0] + accum_quat[1] * accum_quat[1] + accum_quat[2]
* accum_quat[2] + accum_quat[3] * accum_quat[3]);
for (int i = 0; i < 4; i++)
{
normalized_quat[i] = accum_quat[i] / quat_norm;
}
double normalized_vector[3];
// Copy to inputs:
for (int i = 0; i < 3; i++)
{
normalized_vector[i] = accum_trans[i] / n_success;
}
// copy what we used to service call response:
resp.success = true;
resp.pose.header.stamp = ros::Time::now();
resp.pose.header.frame_id = tf_ref_frame_id;
resp.pose.pose.position.x = normalized_vector[0];
resp.pose.pose.position.y = normalized_vector[1];
resp.pose.pose.position.z = normalized_vector[2];
resp.pose.pose.orientation.w = normalized_quat[0];
resp.pose.pose.orientation.x = normalized_quat[1];
resp.pose.pose.orientation.y = normalized_quat[2];
resp.pose.pose.orientation.z = normalized_quat[3];
updateTimer.start();
return true;
}
int main(int argc, char** argv)
{
ROS_INFO("Started fullbody_teleop.");
ros::init(argc, argv, "fullbody_teleop");
ros::NodeHandle nh;
std::string robotDescription;
if (!nh.getParam("/robot_description", robotDescription))
{
ROS_FATAL("Parameter for robot description not provided");
}
huboModel.initString(robotDescription);
//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );
for (auto jointPair : huboModel.joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointPair.second;
if (joint->name[1] == 'F') { continue; }
if (joint->name== "TSY") { continue; }
makeJointMarker(joint->name);
}
std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;
ros::Duration(0.1).sleep();
for (int i = 0; i < HUBO_JOINT_COUNT; i++)
{
if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
{
planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
}
for (int side = 0; side < 2; side++)
for (int i = 1; i <= 3; i++)
for (int j = 1; j <= 3; j++)
{
std::string sideStr = (side == 0) ? "R" : "L";
planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
makeSaveButton();
gIntServer->applyChanges();
gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);
gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
gTimer.start();
ros::spin();
gIntServer.reset();
return 0;
}
/********** callback for the controller **********/
void joy_callback(const sensor_msgs::Joy& msg)
{
watchdogTimer.stop();
command_from_xbox = geometry_msgs::Twist();// cleaning the xbox twist message
a_button = msg.buttons[0];// a button lands
if (a_button > 0)
{
land_pub.publish(std_msgs::Empty());
}
x_button = msg.buttons[2];// x button sends constant
if (x_button > 0)
{
send_0 = true;
}
b_button = msg.buttons[1];// b button kills motors
if (b_button > 0)
{
reset_pub.publish(std_msgs::Empty());
}
y_button = msg.buttons[3];// y button takes off
if (y_button > 0)
{
takeoff_pub.publish(std_msgs::Empty());
}
dpad_l = msg.buttons[11];//dpad left value, fine adjustment of tilt angle negatively
dpad_r = msg.buttons[12];//dpad right value, fine adjustment of tilt angle positively
dpad_u = msg.buttons[13];//dpad up value, coarse adjustment of tilt angle positively
dpad_d = msg.buttons[14];//dpad down value, coarse adjustment of tilt angle negatively
int tilt_neg = -1*dpad_l - 10*dpad_d;// tilt negatively
int tilt_pos = 1*dpad_r + 10*dpad_u;// tilt positively
gimbal_state_desired.angular.y = gimbal_state_desired.angular.y + tilt_neg + tilt_pos;// adjust the gimbal
camera_state_pub.publish(gimbal_state_desired);// update the angle
left_bumper = msg.buttons[4];// left bumper for using the tracking as the controller output
right_bumper = msg.buttons[5];// right bumper for using the mocap as the controller output
start_autonomous = (left_bumper > 0) || (right_bumper > 0);// start the autonomous if either are greater than 0
right_stick_vert = joy_deadband(msg.axes[4]);// right thumbstick vert controls linear x
command_from_xbox.linear.x = joy_gain*right_stick_vert;
right_stick_horz = joy_deadband(msg.axes[3]);// right thumbstick horz controls linear y
command_from_xbox.linear.y = joy_gain*right_stick_horz;
left_stick_vert = joy_deadband(msg.axes[1]);// left thumbstick vert controls linear z
command_from_xbox.linear.z = joy_gain*left_stick_vert;
left_stick_horz = joy_deadband(msg.axes[0]);// left thumbstick horz controls angular z
command_from_xbox.angular.z = -1*joy_gain*left_stick_horz;
if (!start_autonomous)// if not using the autonomous velocities as output will indicate recieved a new control input
{
if (send_0)
{
//command_from_xbox.linear.x = 0;
//command_from_xbox.linear.y = 0;
//command_from_xbox.linear.z = 0;
//command_from_xbox.angular.z = 0;
send_0 = false;
}
recieved_command_from_xbox = true;
}
watchdogTimer.start();
}