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);
}
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.");
}
}
/**
* 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 *");
}
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 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 dynConfigCallback(Config &cfg, uint32_t level) {
boost::mutex::scoped_lock lock(cfg_mutex_);
x_acc_lim_ = cfg.x_acc_lim;
y_acc_lim_ = cfg.y_acc_lim;
yaw_acc_lim_ = cfg.yaw_acc_lim;
interpolate_max_frame_ = cfg.interpolate_max_frame;
if(desired_rate_ != cfg.desired_rate) {
desired_rate_ = cfg.desired_rate;
if (timer_.isValid()) {
ros::Duration d = timerDuration();
timer_.setPeriod(d);
ROS_INFO_STREAM("timer loop rate is changed to " << 1.0 / d.toSec() << "[Hz]");
}
}
}
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();
}
void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
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();
}
void RosAriaNode::sonarDisconnectCb()
{
if (!robot->tryLock()) {
ROS_ERROR("Skipping sonarConnectCb because could not lock");
return;
}
if (robot->areSonarsEnabled())
{
robot->disableSonar();
sonar_tf_timer.stop();
}
robot->unlock();
}
void callback(octoflex::OctoFlexConfig &config, uint32_t level)
{
checkFullVolume_ = config.checkFullVolume;
simTime_ = config.forwardTime;
numSteps_ = config.numberOfSteps;
length_ = config.length;
width_ = config.width;
height_ = config.height;
originOffsetX_ = config.originOffsetX;
originOffsetY_ = config.originOffsetY;
originOffsetZ_ = config.originOffsetZ;
resolution_ = config.resolution;
rate_ = config.rate;
visPause_ = config.visualizationPause;
loopTimer_.setPeriod(ros::Duration(1.0/rate_));
}
void TimerCallback(const ros::TimerEvent&) {
aero_srr_msgs::ObjectLocationMsg test_msg;
test_msg.pose.pose.position.x = x_pos;
test_msg.pose.pose.position.y = y_pos;
test_msg.pose.pose.position.z = z_pos;
tf::Quaternion q;
q.setRPY(rx_pos, ry_pos, rz_pos);
tf::quaternionTFToMsg(q, test_msg.pose.pose.orientation);
test_msg.header.frame_id = "/arm_mount";
test_msg.pose.header.frame_id = test_msg.header.frame_id;
test_msg.header.stamp = ros::Time::now();
test_msg.pose.header.stamp = ros::Time::now();
pub.publish(test_msg);
timer.stop();
}
void handle_autopilot_version(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_autopilot_version_t apv;
mavlink_msg_autopilot_version_decode(msg, &apv);
autopilot_version_timer.stop();
uas->update_capabilities(true, apv.capabilities);
// Note based on current APM's impl.
// APM uses custom version array[8] as a string
ROS_INFO_NAMED("sys", "VER: Capabilities 0x%016llx", (long long int)apv.capabilities);
ROS_INFO_NAMED("sys", "VER: Flight software: %08x (%*s)",
apv.flight_sw_version,
8, apv.flight_custom_version);
ROS_INFO_NAMED("sys", "VER: Middleware software: %08x (%*s)",
apv.middleware_sw_version,
8, apv.middleware_custom_version);
ROS_INFO_NAMED("sys", "VER: OS software: %08x (%*s)",
apv.os_sw_version,
8, apv.os_custom_version);
ROS_INFO_NAMED("sys", "VER: Board hardware: %08x", apv.board_version);
ROS_INFO_NAMED("sys", "VER: VID/PID: %04x:%04x", apv.vendor_id, apv.product_id);
ROS_INFO_NAMED("sys", "VER: UID: %016llx", (long long int)apv.uid);
}
/*!
* @param cmd_vel_msg Received message from topic
*/
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_vel_msg)
{
if(autonomus_mode_) // Only if we are in autonomous mode
{
// Lock proxy to avoid interfering with laser scan reading
boost::mutex::scoped_lock lock(proxy_lock_);
ROS_DEBUG("Sent (vx,vtheta) [%f %f]", cmd_vel_msg->linear.x, cmd_vel_msg->angular.z);
if (cmd_vel_msg->linear.x == 0.0) //If stopping
{
soft_stop(cmd_vel_msg->angular.z);
}
//ROS_DEBUG("Received cmd_vel msg [linear.x linear.y angular.z] [%f %f %f]", cmd_vel_msg->linear.x , cmd_vel_msg->linear.y,cmd_vel_msg->angular.z);
// float vel_linear = cmd_vel_msg->linear.x;
// Send command to wheelchair
last_linear_vel_ = cmd_vel_msg->linear.x;
proxy_.motionSetSpeed(cmd_vel_msg->linear.x, cmd_vel_msg->angular.z);
watchdog_timer.stop();
if(watchdog_duration > 0)
watchdog_timer = node_handle_.createTimer(ros::Duration(watchdog_duration), &BBRobotNode::watchdog_callback, this, true);
}
}
int main(int argc, char **argv) {
int mobot_count = 1; //counter for number of mobots working
if (argv[1] != NULL) {
if (atoi(argv[1]) == 2)
mobot_count = 2;
if (atoi(argv[1]) >= 3)
mobot_count = 3;
}
//initialising ROS
ros::init(argc, argv, "path_planner");
ros::NodeHandle nh;
nh_ = &nh;
//initialising Mobot 1
ros::Subscriber infraredScan_1 = nh.subscribe("/mobot0/infrared", 1, irCallback1);
ros::Subscriber userInput_1 = nh.subscribe("/mobot0/waypoint_user", 20, userCallback);
ros::Subscriber userInput = nh.subscribe("/path_planner/waypoint_user", 20, userInputCallback);
nextPoseRel_1 = nh.advertise<mobots_msgs::Pose2DPrio> ("/mobot0/waypoint_rel", 20);
mobots[0].id = 1;
mobots[0].theta = 0.0;
mobots[0].x = 0.0;
mobots[0].y = 0.0;
mobots[0].obstacle = false;
mobots[0].userControlled = 0;
mobots[0].timer = 0;
timerMobot_1 = nh.createTimer(ros::Duration(1), timerCallback1);
#if 0
if(mobot_count > 1){ //initialising Mobot 2
ros::Subscriber infraredScan_2 = nh.subscribe("/mobot2/infrared", 1, irCallback2);
ros::Subscriber userInput_2 = nh.subscribe("/mobot2/waypoint_user", 20, userCallback);
nextPoseRel_2 = nh.advertise<mobots_msgs::Pose2DPrio> ("/mobot2/waypoint_rel", 20);
mobots[1].id = 2;
mobots[1].theta = 0.0;
mobots[1].x = 0.0;
mobots[1].y = 0.0;
mobots[1].obstacle = false;
mobots[1].userControlled = 0;
mobots[1].timer = 0;
timerMobot_2 = nh.createTimer(ros::Duration(1), timerCallback2);
}
if(mobot_count > 2){ //initialising Mobot 3
ros::Subscriber infraredScan_3 = nh.subscribe("/mobot3/infrared", 1, irCallback3);
ros::Subscriber userInput_3 = nh.subscribe("/mobot3/waypoint_user", 20, userCallback);
nextPoseRel_3 = nh.advertise<mobots_msgs::Pose2DPrio> ("/mobot3/waypoint_rel", 20);
mobots[2].id = 3;
mobots[2].theta = 0.0;
mobots[2].x = 0.0;
mobots[2].y = 0.0;
mobots[2].obstacle = false;
mobots[2].userControlled = 0;
mobots[2].timer = 0;
timerMobot_3 = nh.createTimer(ros::Duration(1), timerCallback3);
}
keyReqServer = nh.advertiseService("/path_planner/keyboard_request", keyReqCallback);
if(argv[2] != NULL){
std::string str = std::string(argv[2]);
if(str == "square"){
exploreSquare(1, 1000);
}
}
/* Entering the event loop. If there is no obstacle or the user is not controlling the
* Mobot, it is driving straight till something occurs. */
while (ros::ok()) {
for (int i = 0; i <= mobot_count-1; i++) {
if (mobots[i].userControlled == 0 && !mobots[i].obstacle) {
moveMobot(mobots[i].id, 1);
}
}
wait(5000, true);
}
//stopping the timers
timerMobot_1.stop();
if(mobot_count > 1) timerMobot_2.stop();
if(mobot_count > 2) timerMobot_3.stop();
return 0;
#endif
ros::spin();
}
void shutdown(){
timer_dsa.stop();
timer_publish.stop();
timer_diag.stop();
nh_.shutdown();
}
/********** 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();
}
// 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();
}
}
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;
}
void PlaceDetector::processImage()
{
if(!currentImage.empty())
{
/* int satLower = 30;
int satUpper = 230;
int valLower = 30;
int valUpper = 230;*/
timer.stop();
// Mat img;
// cv::cvtColor(currentImage,img,CV_BGR2HSV);
Mat hueChannel= ImageProcess::generateChannelImage(currentImage,0,satLower,satUpper,valLower,valUpper);
Mat hueChannelFiltered;
cv::medianBlur(hueChannel, hueChannelFiltered,3);
vector<bubblePoint> hueBubble = bubbleProcess::convertGrayImage2Bub(hueChannelFiltered,focalLengthPixels,180);
vector<bubblePoint> reducedHueBubble = bubbleProcess::reduceBubble(hueBubble);
/************************** Perform filtering and obtain resulting mat images ***********************/
// Mat satChannel= ImageProcess::generateChannelImage(img,1,satLower,satUpper,valLower,valUpper);
Mat valChannel= ImageProcess::generateChannelImage(currentImage,2,satLower,satUpper,valLower,valUpper);
/*****************************************************************************************************/
/*************************** Convert images to bubbles ***********************************************/
// vector<bubblePoint> satBubble = bubbleProcess::convertGrayImage2Bub(satChannel,focalLengthPixels,255);
vector<bubblePoint> valBubble = bubbleProcess::convertGrayImage2Bub(valChannel,focalLengthPixels,255);
/*****************************************************************************************************/
/***************** Reduce the bubbles ********************************************************/
// vector<bubblePoint> reducedSatBubble = bubbleProcess::reduceBubble(satBubble);
vector<bubblePoint> reducedValBubble = bubbleProcess::reduceBubble(valBubble);
// Calculate statistics
// bubbleStatistics statsHue = bubbleProcess::calculateBubbleStatistics(reducedHueBubble,180);
// bubbleStatistics statsSat = bubbleProcess::calculateBubbleStatistics(reducedSatBubble,255);
bubbleStatistics statsVal = bubbleProcess::calculateBubbleStatistics(reducedValBubble,255);
qDebug()<<"Bubble statistics: "<<statsVal.mean<<statsVal.variance;
currentBasePoint.avgVal = statsVal.mean;
currentBasePoint.varVal = statsVal.variance;
currentBasePoint.id = image_counter;
QString imagefilePath = imagesPath;
imagefilePath.append("/rgb_");
imagefilePath.append(QString::number(image_counter)).append(".jpg");
imwrite(imagefilePath.toStdString().data(),currentImage);
//imwrite()
currentBasePoint.status = 0;
/*********************** WE CHECK FOR THE UNINFORMATIVENESS OF THE FRAME *************************/
if(statsVal.mean <= this->tau_val_mean || statsVal.variance <= this->tau_val_var)
{
// qDebug()<<"This image is uninformative"<<image_counter;
currentBasePoint.status = 1;
// this->shouldProcess = true;
// If we don't have an initialized window then initialize
if(!this->tempwin)
{
this->tempwin = new TemporalWindow();
this->tempwin->tau_n = this->tau_n;
this->tempwin->tau_w = this->tau_w;
this->tempwin->startPoint = image_counter;
this->tempwin->endPoint = image_counter;
this->tempwin->id = twindow_counter;
this->tempwin->members.push_back(currentBasePoint);
}
else
{
this->tempwin->endPoint = image_counter;
this->tempwin->members.push_back(currentBasePoint);
}
/// dbmanager.insertBasePoint(currentBasePoint);
wholebasepoints.push_back(currentBasePoint);
// previousBasePoint = currentBasePoint;
image_counter++;
detector.currentImage.release();
// timer.start();
detector.shouldProcess = true;
return;
}
/*********************************** IF THE FRAME IS INFORMATIVE *************************************************/
else
{
Mat totalInvariants;
qint64 start = QDateTime::currentMSecsSinceEpoch();
DFCoefficients dfcoeff = bubbleProcess::calculateDFCoefficients(reducedHueBubble,noHarmonics,noHarmonics);
Mat hueInvariants = bubbleProcess::calculateInvariantsMat(dfcoeff,noHarmonics, noHarmonics);
// totalInvariants = hueInvariants.clone();
cv::Mat logTotal;
// qDebug()<<hueInvariants.rows<<hueInvariants.cols<<hueInvariants.at<float>(0,10);
Mat grayImage;
cv::cvtColor(currentImage,grayImage,CV_BGR2GRAY);
std::vector<Mat> sonuc = ImageProcess::applyFilters(grayImage);
for(uint j = 0; j < sonuc.size(); j++)
{
vector<bubblePoint> imgBubble = bubbleProcess::convertGrayImage2Bub(sonuc[j],focalLengthPixels,255);
vector<bubblePoint> resred = bubbleProcess::reduceBubble(imgBubble);
DFCoefficients dfcoeff = bubbleProcess::calculateDFCoefficients(resred,noHarmonics,noHarmonics);
Mat invariants= bubbleProcess::calculateInvariantsMat(dfcoeff,noHarmonics,noHarmonics);
if(j==0)
totalInvariants = invariants.clone();
else
cv::hconcat(totalInvariants, invariants, totalInvariants);
}
// qDebug()<<totalInvariants.at<float>(0,64);
cv::log(totalInvariants,logTotal);
logTotal = logTotal/25;
cv::transpose(logTotal,logTotal);
qint64 stop = QDateTime::currentMSecsSinceEpoch();
qDebug()<<"Bubble time"<<(stop-start);
// TOTAL INVARIANTS N X 1 vector
for(int kk = 0; kk < logTotal.rows; kk++)
{
if(logTotal.at<float>(kk,0) < 0)
logTotal.at<float>(kk,0) = 0.5;
}
// qDebug()<<logTotal.rows<<logTotal.cols<<logTotal.at<float>(10,0);
// We don't have a previous base point
if(previousBasePoint.id == 0)
{
currentBasePoint.id = image_counter;
currentBasePoint.invariants = logTotal;
previousBasePoint = currentBasePoint;
currentPlace->members.push_back(currentBasePoint);
/// dbmanager.insertBasePoint(currentBasePoint);
wholebasepoints.push_back(currentBasePoint);
}
else
{
currentBasePoint.id = image_counter;
currentBasePoint.invariants = logTotal;
// double result = compareHistHK(currentBasePoint.invariants,previousBasePoint.invariants, CV_COMP_CHISQR);
// MY VERSION FOR CHISQR, SIMPLER!!
double result= compareHKCHISQR(currentBasePoint.invariants,previousBasePoint.invariants);
qDebug()<<"Invariant diff between "<<currentBasePoint.id<<previousBasePoint.id<<"is"<<result;
// qDebug()<<"Invariant diff between "<<currentBasePoint.id<<previousBasePoint.id<<"is"<<result<<result2;//previousBasePoint.invariants.rows<<currentBasePoint.invariants.rows;
// qDebug()<<currentBasePoint.invariants.at<float>(1,0)<<currentBasePoint.invariants.at<float>(2,0)<<previousBasePoint.invariants.at<float>(1,0)<<previousBasePoint.invariants.at<float>(2,0);
// JUST FOR DEBUGGING-> WRITES INVARIANT TO THE HOME FOLDER
// writeInvariant(previousBasePoint.invariants,previousBasePoint.id);
///////////////////////////// IF THE FRAMES ARE COHERENT ///////////////////////////////////////////////////////////////////////////////////////////////////////
if(result <= tau_inv && result > 0)
{
/// dbmanager.insertBasePoint(currentBasePoint);
wholebasepoints.push_back(currentBasePoint);
/// If we have a temporal window
if(tempwin)
{
// Temporal window will extend, we are still looking for the next incoming frames
if(tempwin->checkExtensionStatus(currentBasePoint.id))
{
tempwin->cohMembers.push_back(currentBasePoint);
basepointReservoir.push_back(currentBasePoint);
}
// Temporal window will not extend anymore, we should check whether it is really a temporal window or not
else
{
float area = this->tempwin->totalDiff/(tempwin->endPoint - tempwin->startPoint+1);
qDebug()<<"Temporal Window Area"<<area;
// This is a valid temporal window
if(tempwin->endPoint - tempwin->startPoint >= tau_w && area>= tau_avgdiff)
{
qDebug()<<"New Place";
currentPlace->calculateMeanInvariant();
qDebug()<<"Current place mean invariant: "<<currentPlace->meanInvariant.rows<<currentPlace->meanInvariant.cols<<currentPlace->meanInvariant.at<float>(50,0);
if(currentPlace->memberIds.rows >= tau_p){
dbmanager.insertPlace(*currentPlace);
std_msgs::Int16 plID;
plID.data = this->placeID;
placedetectionPublisher.publish(plID);
this->detectedPlaces.push_back(*currentPlace);
this->placeID++;
}
delete currentPlace;
currentPlace = 0;
// this->placeID++;
/* cv::Mat result = DatabaseManager::getPlaceMeanInvariant(this->placeID-1);
qDebug()<<"Previous place mean invariant: "<<result.rows<<result.cols<<result.at<float>(50,0);
result = DatabaseManager::getPlaceMemberIds(this->placeID-1);
for(int k = 0; k< result.rows; k++){
qDebug()<<"Previous place members: "<<result.rows<<result.cols<<result.at<unsigned short>(k,0);
}*/
currentPlace = new Place(this->placeID);
// currentPlace->
basepointReservoir.push_back(currentBasePoint);
currentPlace->members = basepointReservoir;
basepointReservoir.clear();
dbmanager.insertTemporalWindow(*tempwin);
delete tempwin;
tempwin = 0;
this->twindow_counter++;
// A new place will be created. Current place will be published
}
// This is just a noisy temporal window. We should add the coherent basepoints to the current place
else
{
basepointReservoir.push_back(currentBasePoint);
delete tempwin;
tempwin = 0;
std::vector<BasePoint> AB;
AB.reserve( currentPlace->members.size() + basepointReservoir.size() ); // preallocate memory
AB.insert( AB.end(), currentPlace->members.begin(), currentPlace->members.end() );
AB.insert( AB.end(), basepointReservoir.begin(), basepointReservoir.end() );
currentPlace->members.clear();
currentPlace->members = AB;
basepointReservoir.clear();
}
}
}
else
{
currentPlace->members.push_back(currentBasePoint);
}
}
///////////////////////// IF THE FRAMES ARE INCOHERENT /////////////////////////////////////
else
{
currentBasePoint.status = 2;
/// dbmanager.insertBasePoint(currentBasePoint);
wholebasepoints.push_back(currentBasePoint);
// If we don't have a temporal window create one
if(!tempwin)
{
tempwin = new TemporalWindow();
this->tempwin->tau_n = this->tau_n;
this->tempwin->tau_w = this->tau_w;
this->tempwin->startPoint = image_counter;
this->tempwin->endPoint = image_counter;
this->tempwin->id = twindow_counter;
this->tempwin->totalDiff +=result;
this->tempwin->members.push_back(currentBasePoint);
}
// add the basepoint to the temporal window
else
{
// Temporal window will extend, we are still looking for the next incoming frames
if(tempwin->checkExtensionStatus(currentBasePoint.id))
{
this->tempwin->endPoint = image_counter;
this->tempwin->members.push_back(currentBasePoint);
this->tempwin->totalDiff +=result;
basepointReservoir.clear();
}
else
{
float avgdiff;
avgdiff = this->tempwin->totalDiff/(tempwin->endPoint - tempwin->startPoint+1);
qDebug()<<"Temporal Window Average Diff"<<avgdiff;
// This is a valid temporal window
if(tempwin->endPoint - tempwin->startPoint >= tau_w && avgdiff >= tau_avgdiff)
{
// float summ = 0;
//Modifikasyon
/* for(int kl = 0; kl < tempwin->members.size(); kl++)
{
BasePoint abasepoint = tempwin->members.at(kl);
abasepoint.
}*/
qDebug()<<"New Place";
currentPlace->calculateMeanInvariant();
qDebug()<<"Current place mean invariant: "<<currentPlace->meanInvariant.rows<<currentPlace->meanInvariant.cols<<currentPlace->meanInvariant.at<float>(50,0);
if(currentPlace->memberIds.rows >= tau_p)
{
dbmanager.insertPlace(*currentPlace);
std_msgs::Int16 plID ;
plID.data = this->placeID;
placedetectionPublisher.publish(plID);
this->detectedPlaces.push_back(*currentPlace);
this->placeID++;
}
delete currentPlace;
currentPlace = 0;
// this->placeID++;
// cv::Mat result = DatabaseManager::getPlaceMeanInvariant(this->placeID-1);
// qDebug()<<"Previous place mean invariant: "<<result.rows<<result.cols<<result.at<float>(50,0);
// result = DatabaseManager::getPlaceMemberIds(this->placeID-1);
/* for(int k = 0; k< result.rows; k++){
qDebug()<<"Previous place members: "<<result.rows<<result.cols<<result.at<unsigned short>(k,0);
}*/
currentPlace = new Place(this->placeID);
// currentPlace->
//basepointReservoir.push_back(currentBasePoint);
currentPlace->members = basepointReservoir;
basepointReservoir.clear();
dbmanager.insertTemporalWindow(*tempwin);
delete tempwin;
tempwin = 0;
this->twindow_counter++;
// A new place will be created. Current place will be published
}
// This is just a noisy temporal window. We should add the coherent basepoints to the current place
else
{
// basepointReservoir.push_back(currentBasePoint);
delete tempwin;
tempwin = 0;
std::vector<BasePoint> AB;
AB.reserve( currentPlace->members.size() + basepointReservoir.size() ); // preallocate memory
AB.insert( AB.end(), currentPlace->members.begin(), currentPlace->members.end() );
AB.insert( AB.end(), basepointReservoir.begin(), basepointReservoir.end() );
currentPlace->members.clear();
currentPlace->members = AB;
basepointReservoir.clear();
}
tempwin = new TemporalWindow();
this->tempwin->tau_n = this->tau_n;
this->tempwin->tau_w = this->tau_w;
this->tempwin->startPoint = image_counter;
this->tempwin->endPoint = image_counter;
this->tempwin->id = twindow_counter;
this->tempwin->members.push_back(currentBasePoint);
}
/* this->tempwin->endPoint = image_counter;
this->tempwin->members.push_back(currentBasePoint);
basepointReservoir.clear();*/
}
}
previousBasePoint = currentBasePoint;
//////////////////////////////////////////////////////////////////////////////////////////////////
}
// DatabaseManager::insertInvariants(HUE_TYPE,frameNumber,invariants);
// qDebug()<<"Image Counter: "<<image_counter;
image_counter++;
//this->shouldProcess = true;
}
}
this->currentImage.release();
this->shouldProcess = true;
// timer.start();
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
robot->lock();
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
//arbitrary non-zero values so dynamic reconfigure isn't STUPID
dynConf_min.trans_vel_max = 0.1;
dynConf_min.rot_vel_max = 0.1;
dynConf_min.trans_accel = 0.1;
dynConf_min.trans_decel = 0.1;
dynConf_min.rot_accel = 0.1;
dynConf_min.rot_decel = 0.1;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0;
dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0;
dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0;
dynConf_default.trans_accel = robot->getTransAccel() / 1000.0;
dynConf_default.trans_decel = robot->getTransDecel() / 1000.0;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180.0;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180.0;
/* ROS_ERROR("ON ROBOT NOW\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());
ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max, dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
RevCount = robot->getOrigRobotConfig()->getRevCount();
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_default);
for(int i = 0; i < 16; i++)
{
sonar_tf_array[i].header.frame_id = frame_id_base_link;
std::stringstream _frame_id;
_frame_id << "sonar" << i;
sonar_tf_array[i].child_frame_id = _frame_id.str();
ArSensorReading* _reading = NULL;
_reading = robot->getSonarReading(i);
sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
sonar_tf_array[i].transform.translation.z = 0.19;
sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
}
for (int i=0;i<16;i++) {
sensor_msgs::Range r;
ranges.data.push_back(r);
}
int i=0,j=0;
if (sonars__crossed_the_streams) {
i=8;
j=8;
}
for(; i<16; i++) {
//populate the RangeArray msg
std::stringstream _frame_id;
_frame_id << "sonar" << i;
ranges.data[i].header.frame_id = _frame_id.str();
ranges.data[i].radiation_type = 0;
ranges.data[i].field_of_view = 0.2618f;
ranges.data[i].min_range = 0.03f;
ranges.data[i].max_range = 5.0f;
}
// Enable the motors
robot->enableMotors();
robot->disableSonar();
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
robot->unlock();
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
for(int i =0; i < 16; i++) {
std::stringstream topic_name;
topic_name << "range" << i;
range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000,
boost::bind(&RosAriaNode::sonarConnectCb,this),
boost::bind(&RosAriaNode::sonarDisconnectCb, this));
}
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
veltime = ros::Time::now();
sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this);
sonar_tf_timer.stop();
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
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;
}
EKF()
{
// Get Parameters
ros::NodeHandle nhp("~");
nhp.param<bool>("deadReckoning", deadReckoning, false);
nhp.param<bool>("artificialSwitching", artificialSwitching, false);
nhp.param<double>("visibilityTimeout", visibilityTimeout, 0.2);
nhp.param<string>("cameraName",cameraName,"camera");
nhp.param<string>("markerID",markerID,"100");
nhp.param<double>("q",q,4.0); // process noise
nhp.param<double>("r",r,4.0); // measurement noise
nhp.param<double>("delTon",delTon,4.0);
nhp.param<double>("delToff",delToff,1.0);
// Initialize states
xhat << 0,0,0.1;
xlast << 0,0,0.1;
lastImageTime = ros::Time::now().toSec();
lastVelTime = lastImageTime;
estimatorOn = true;
gotCamParam = false;
// Initialize EKF matrices
Q = q*Matrix3d::Identity();
R = r*Matrix2d::Identity();
H << 1,0,0,
0,1,0;
// Get camera parameters
cout << cameraName+"/camera_info" << endl;
camInfoSub = nh.subscribe(cameraName+"/camera_info",1,&EKF::camInfoCB,this);
ROS_DEBUG("Waiting for camera parameters on topic ...");
do {
ros::spinOnce();
ros::Duration(0.1).sleep();
} while (!(ros::isShuttingDown()) and !gotCamParam);
ROS_DEBUG("Got camera parameters");
// Output publishers
outputPub = nh.advertise<switch_vis_exp::Output>("output",10);
pointPub = nh.advertise<geometry_msgs::PointStamped>("output_point",10);
// Subscribers for feature and velocity data
if (deadReckoning)
{
targetVelSub = nh.subscribe("ugv0/odom",1,&EKF::targetVelCBdeadReckoning,this);
}
else
{
targetVelSub = nh.subscribe("ugv0/body_vel",1,&EKF::targetVelCBmocap,this);
}
camVelSub = nh.subscribe("image/body_vel",1,&EKF::camVelCB,this);
featureSub = nh.subscribe("markerCenters",1,&EKF::featureCB,this);
// Switching
if (artificialSwitching)
{
switchingTimer = nh.createTimer(ros::Duration(delTon),&EKF::switchingTimerCB,this,true);
}
else
{
// Initialize watchdog timer for feature visibility check
watchdogTimer = nh.createTimer(ros::Duration(visibilityTimeout),&EKF::timeout,this,true);
watchdogTimer.stop(); // Dont start watchdog until feature first visible
}
}
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());
}
}