void VRPN_CALLBACK VrpnTrackerRos::handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose)
{
VrpnTrackerRos *tracker = static_cast<VrpnTrackerRos *>(userData);
ros::Publisher *pose_pub;
std::size_t sensor_index(0);
ros::NodeHandle nh = tracker->output_nh_;
if (tracker->process_sensor_id_)
{
sensor_index = static_cast<std::size_t>(tracker_pose.sensor);
nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_pose.sensor));
}
if (tracker->pose_pubs_.size() <= sensor_index)
{
tracker->pose_pubs_.resize(sensor_index + 1);
}
pose_pub = &(tracker->pose_pubs_[sensor_index]);
if (pose_pub->getTopic().empty())
{
*pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose", 1);
}
if (pose_pub->getNumSubscribers() > 0)
{
if (tracker->use_server_time_)
{
tracker->pose_msg_.header.stamp.sec = tracker_pose.msg_time.tv_sec;
tracker->pose_msg_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000;
}
else
{
tracker->pose_msg_.header.stamp = ros::Time::now();
}
tracker->pose_msg_.pose.position.x = tracker_pose.pos[0];
tracker->pose_msg_.pose.position.y = tracker_pose.pos[1];
tracker->pose_msg_.pose.position.z = tracker_pose.pos[2];
tracker->pose_msg_.pose.orientation.x = tracker_pose.quat[0];
tracker->pose_msg_.pose.orientation.y = tracker_pose.quat[1];
tracker->pose_msg_.pose.orientation.z = tracker_pose.quat[2];
tracker->pose_msg_.pose.orientation.w = tracker_pose.quat[3];
pose_pub->publish(tracker->pose_msg_);
}
if (tracker->broadcast_tf_)
{
static tf2_ros::TransformBroadcaster tf_broadcaster;
if (tracker->use_server_time_)
{
tracker->transform_stamped_.header.stamp.sec = tracker_pose.msg_time.tv_sec;
tracker->transform_stamped_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000;
}
else
{
tracker->transform_stamped_.header.stamp = ros::Time::now();
}
if (tracker->process_sensor_id_)
{
tracker->transform_stamped_.child_frame_id = tracker->tracker_name + "/" + std::to_string(tracker_pose.sensor);
}
else
{
tracker->transform_stamped_.child_frame_id = tracker->tracker_name;
}
tracker->transform_stamped_.transform.translation.x = tracker_pose.pos[0];
tracker->transform_stamped_.transform.translation.y = tracker_pose.pos[1];
tracker->transform_stamped_.transform.translation.z = tracker_pose.pos[2];
tracker->transform_stamped_.transform.rotation.x = tracker_pose.quat[0];
tracker->transform_stamped_.transform.rotation.y = tracker_pose.quat[1];
tracker->transform_stamped_.transform.rotation.z = tracker_pose.quat[2];
tracker->transform_stamped_.transform.rotation.w = tracker_pose.quat[3];
tf_broadcaster.sendTransform(tracker->transform_stamped_);
}
}
void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_local_position_ned_t pos_ned;
mavlink_msg_local_position_ned_decode(msg, &pos_ned);
auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
auto orientation = uas->get_attitude_orientation();
auto pose = boost::make_shared<geometry_msgs::PoseStamped>();
pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);
tf::pointEigenToMsg(position, pose->pose.position);
// XXX FIX ME #319
tf::quaternionTFToMsg(orientation, pose->pose.orientation);
local_position.publish(pose);
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
transform.transform.rotation = pose->pose.orientation;
tf::vectorEigenToMsg(position, transform.transform.translation);
tf2_broadcaster.sendTransform(transform);
}
}
nav_msgs::Odometry send_odom(void)
{
cur_time = Time::now();
double dt = cur_time.toSec() - send_time.toSec();
odom_x += vel.linear.x * cos(odom_theta) * dt;
odom_y += vel.linear.x * sin(odom_theta) * dt;
odom_theta += vel.angular.z * dt;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odom_theta);
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = cur_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = odom_x;
odom_trans.transform.translation.y = odom_y;
odom_trans.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, odom_theta);
odom_trans.transform.rotation.x = q.x();
odom_trans.transform.rotation.y = q.y();
odom_trans.transform.rotation.z = q.z();
odom_trans.transform.rotation.w = q.w();
br.sendTransform(odom_trans);
nav_msgs::Odometry odom;
odom.header.stamp = cur_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = odom_x;
odom.pose.pose.position.y = odom_y;
odom.pose.pose.position.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = vel.linear.x;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = vel.angular.z;
send_time = cur_time;
return odom;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mouse_odom_node");
ros::NodeHandle node;
ros::Rate loop_rate(1000);
//ros parameters
std::string dev[2];
ros::param::param<std::string>("~left_dev_event", dev[L], "/dev/input/event3"); //left mouse
ros::param::param<std::string>("~right_dev_event", dev[R], "/dev/input/event6"); //right mouse
std::string frame[2];
ros::param::param<std::string>("~left_frame_id", frame[L], "left_mouse");
ros::param::param<std::string>("~right_frame_id", frame[R], "right_mouse");
float cpi;
ros::param::param<float>("~counts_per_meter", cpi, 800);
//publishing odom
ros::Publisher odom_pub = node.advertise<nav_msgs::Odometry>("mouse/odom", 1000);
// //transforms
// tf::StampedTransform base_to_front_mouse;
// tf::StampedTransform base_to_left_mouse;
// tf::TransformListener f_listener, l_listener;
// ros::Duration(5.0).sleep();
// try{
// f_listener.lookupTransform("base_link", "front_mouse", ros::Time(0), base_to_front_mouse);
// }
// catch (tf::TransformException ex){
// ROS_ERROR("%s",ex.what());
// ros::Duration(1.0).sleep();
// }
// try{
// l_listener.lookupTransform("base_link", "left_mouse", ros::Time(0), base_to_left_mouse);
// }
// catch (tf::TransformException ex){
// ROS_ERROR("%s",ex.what());
// ros::Duration(1.0).sleep();
// }
// tf::Vector3 front_mouse_vector = base_to_front_mouse.getOrigin();
// tf::Vector3 left_mouse_vector = base_to_left_mouse.getOrigin();
// ROS_WARN("origin: x %d, y %d", (int)(front_mouse_vector.x()*100), (int)(front_mouse_vector.y()*100));
// ROS_WARN("origin: x %d, y %d", (int)(left_mouse_vector.x()*100), (int)(left_mouse_vector.y()*100));
// //KDL PARSER
// KDL::Tree my_tree;
// std::string robot_desc_string;
// ros::param::param<std::string>("robot_description", robot_desc_string, "string()");
// if (!kdl_parser::treeFromString(robot_desc_string, my_tree))
// {
// ROS_ERROR("Failed to construct kdl tree");
// return false;
// }
//transformations
static tf2_ros::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_to_base;
odom_to_base.header.frame_id = "odom";
odom_to_base.child_frame_id = "base_link";
odom_to_base.header.stamp = ros::Time::now();
odom_to_base.transform.translation.x = 0;
odom_to_base.transform.translation.y = 0;
odom_to_base.transform.translation.z = 0;
odom_to_base.transform.rotation = tf::createQuaternionMsgFromYaw(0);
ros::Time start_time = ros::Time::now();
MouseOdom left_mouse(dev[L], frame[L]);
MouseOdom right_mouse(dev[R], frame[R]);
left_mouse.openDevice();
right_mouse.openDevice();
geometry_msgs::Vector3Stamped data, data2;
nav_msgs::Odometry odom;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = 0.0;
odom.pose.pose.position.y = 0.0;
float th =0;
float scale = cpi/0.0254; //multiply this with counts to take meters
while(ros::ok())
{
if(left_mouse.readRelativeMove() || right_mouse.readRelativeMove())
{
data = left_mouse.getVector();
data2 = right_mouse.getVector();
ros::Time data_stamp;
//Stamp data based on latest data from mouses
if (data2.header.stamp.toSec() > data.header.stamp.toSec())
data_stamp = data2.header.stamp;
else
data_stamp = data.header.stamp;
//ROS_INFO("mouse real data stamp %d.%d", data_stamp.sec, data_stamp.nsec);
//ROS_INFO(" ros::Time data stamp %d.%d", ros::Time::now().sec, ros::Time::now().nsec);
//calculation
odom.header.stamp = data_stamp;
th += atan2((-data.vector.x + data2.vector.x), (0.200*scale));
odom.pose.pose.position.x += (((data.vector.x + data2.vector.x)/2)*cos(th) + ((data.vector.y + data2.vector.y)/2)*sin(th)) /scale;
odom.pose.pose.position.y += (((data.vector.x + data2.vector.x)/2)*sin(th) + ((data.vector.y + data2.vector.y)/2)*cos(th)) /scale;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
odom.pose.pose.orientation = odom_quat;
//ROS_INFO("odom: x %f y %f th %f", odom.pose.pose.position.x, odom.pose.pose.position.y, th);
odom_pub.publish(odom);
odom_to_base.header.stamp = data_stamp;
odom_to_base.transform.translation.x = odom.pose.pose.position.x;
odom_to_base.transform.translation.y = odom.pose.pose.position.y;
odom_to_base.transform.translation.z = 0.0;
odom_to_base.transform.rotation = odom.pose.pose.orientation;
odom_broadcaster.sendTransform(odom_to_base);
}
else
{
// No odometry data broadcasting
// Keep posting transformation data in order to work with camera
// ROS_INFO("Time data stamp %d.%d", ros::Time::now().sec, ros::Time::now().nsec);
odom_to_base.header.stamp = ros::Time::now();
odom_broadcaster.sendTransform(odom_to_base);
}
ros::spinOnce();
loop_rate.sleep();
}
left_mouse.closeDevice();
right_mouse.closeDevice();
return 0;
}
/////////////////
/// 消息处理函数
/////////////////
void UWBDriver::handle_dist_msg(const std::string &line){
std::string str(line);
int anchor_id = 0;
static int max_anchor_id = 0;
double dist = 0.0;
std::string tmp_dist;
// Max anchor num '6'
// std::vector<double> dist_vec(6, 0.0);
dist_vec.resize(6);
// for (int i=0; i< dist_vec.size(); i++)
// dist_vec[i] = 0.0;
cal_dist.resize(2); // (x,y)
// Only process "distance..." lines
if (str.find("distance") != std::string::npos)
{
// get anchor
anchor_id = str[9]-'0'; // char to int
max_anchor_id = anchor_id>max_anchor_id ? anchor_id: max_anchor_id;
// get dist
for (int i=11; i<18; i++)
{
tmp_dist += str[i];
}
// str to float && 'mm' to 'm'
dist = std::atof(tmp_dist.c_str())/1000.0;
// push back dist
dist_vec[anchor_id-1] = dist;
// for test
// std::cout << "str[9]: " << str[9] << ", anchor_id: " << anchor_id << std::endl
// << "tmp_dist:" << tmp_dist << ", dist: " << dist << std::endl;
std::cout << "dist_vec.size: " << dist_vec.size() << std::endl;
for (int i = 0; i < dist_vec.size(); i ++)
std::cout << "dist_vec[" << i << "]: " << dist_vec[i] << std::endl;
}
// Send tf
if (anchor_id == max_anchor_id)
{
/*****************TO DO***********************************/
// cal the real dist
cal_dist[0] = dist_vec[0]; // x
cal_dist[1] = 0.0; // y
// Send tf
transformStamped_.header.stamp = ros::Time::now();
// std::string frame_id = "anchor_" + std::to_string(0+1);
transformStamped_.header.frame_id = base_frame_;
transformStamped_.child_frame_id = "tag_0";
transformStamped_.transform.translation.x = cal_dist[0];
transformStamped_.transform.translation.y = cal_dist[1];
transformStamped_.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, 0);
transformStamped_.transform.rotation.x = q.x();
transformStamped_.transform.rotation.y = q.y();
transformStamped_.transform.rotation.z = q.z();
transformStamped_.transform.rotation.w = q.w();
br_.sendTransform(transformStamped_);
// for reuse
dist_vec.clear();
}
}
void FiducialsNode::location_cb(int id, double x, double y, double z,
double bearing) {
ROS_INFO("location_announce:id=%d x=%f y=%f bearing=%f",
id, x, y, bearing * 180. / 3.1415926);
visualization_msgs::Marker marker = createMarker(position_namespace, id);
ros::Time now = marker.header.stamp;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose = scale_position(x, y, z, bearing);
marker.scale.x = 0.2 / scale;
marker.scale.y = 0.05 / scale;
marker.scale.z = 0.05 / scale;
marker.color = position_color;
marker.lifetime = ros::Duration();
marker_pub->publish(marker);
// TODO: subtract out odometry position, and publish transform from
// map to odom
double tf_x = marker.pose.position.x;
double tf_y = marker.pose.position.y;
double tf_yaw = bearing;
// publish a transform based on the position
if( use_odom ) {
// if we're using odometry, look up the odom transform and subtract it
// from our position so that we can publish a map->odom transform
// such that map->odom->base_link reports the correct position
std::string tf_err;
if( tf_buffer.canTransform(pose_frame, odom_frame, now,
ros::Duration(0.1), &tf_err ) ) {
// get odometry position from TF
tf2::Quaternion tf_quat;
tf_quat.setRPY(0.0, 0.0, tf_yaw);
tf2::Transform pose(tf_quat, tf2::Vector3(tf_x, tf_y, 0));
// look up camera transform if we can
if( last_camera_frame.length() > 0 ) {
if( tf_buffer.canTransform(pose_frame, last_camera_frame, now,
ros::Duration(0.1), &tf_err) ) {
geometry_msgs::TransformStamped camera_tf;
camera_tf = tf_buffer.lookupTransform(pose_frame,
last_camera_frame, now);
tf2::Transform camera = msg_to_tf(camera_tf);
pose = pose * camera.inverse();
} else {
ROS_ERROR("Cannot look up transform from %s to %s: %s",
pose_frame.c_str(), last_camera_frame.c_str(), tf_err.c_str());
}
}
geometry_msgs::TransformStamped odom;
odom = tf_buffer.lookupTransform(odom_frame, pose_frame, now);
tf2::Transform odom_tf = msg_to_tf(odom);
// M = C * O
// C^-1 * M = O
// C^-1 = O * M-1
tf2::Transform odom_correction = (odom_tf * pose.inverse()).inverse();
geometry_msgs::TransformStamped transform;
tf2::Vector3 odom_correction_v = odom_correction.getOrigin();
transform.transform.translation.x = odom_correction_v.getX();
transform.transform.translation.y = odom_correction_v.getY();
transform.transform.translation.z = odom_correction_v.getZ();
tf2::Quaternion odom_correction_q = odom_correction.getRotation();
transform.transform.rotation.x = odom_correction_q.getX();
transform.transform.rotation.y = odom_correction_q.getY();
transform.transform.rotation.z = odom_correction_q.getZ();
transform.transform.rotation.w = odom_correction_q.getW();
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = odom_frame;
//tf2::transformTF2ToMsg(odom_correction, transform, now, world_frame,
//odom_frame);
if (publish_tf)
tf_pub.sendTransform(transform);
} else {
ROS_ERROR("Can't look up base transform from %s to %s: %s",
pose_frame.c_str(),
odom_frame.c_str(),
tf_err.c_str());
}
} else {
// we're publishing absolute position
geometry_msgs::TransformStamped transform;
transform.header.stamp = now;
transform.header.frame_id = world_frame;
transform.child_frame_id = pose_frame;
transform.transform.translation.x = tf_x;
transform.transform.translation.y = tf_y;
transform.transform.translation.z = 0.0;
transform.transform.rotation = tf::createQuaternionMsgFromYaw(tf_yaw);
if (publish_tf)
tf_pub.sendTransform(transform);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mouse_odom_node", ros::init_options::NoSigintHandler);
signal(SIGINT, ctrl_C_Handler);
ros::NodeHandle node;
ros::Rate loop_rate(1000);
//ros parameters
std::string dev[2];
ros::param::param<std::string>("~left_dev_event", dev[L], "/dev/input/event3"); //left mouse
ros::param::param<std::string>("~right_dev_event", dev[R], "/dev/input/event6"); //right mouse
std::string frame[2];
ros::param::param<std::string>("~left_frame_id", frame[L], "left_mouse");
ros::param::param<std::string>("~right_frame_id", frame[R], "right_mouse");
double cpi;
ros::param::param<double>("~counts_per_meter", cpi, 800);
//publishing odom
ros::Publisher odom_pub = node.advertise<nav_msgs::Odometry>("mouse/odom", 1000);
//transformations
static tf2_ros::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_to_base;
odom_to_base.header.frame_id = "odom";
odom_to_base.child_frame_id = "base_link";
odom_to_base.header.stamp = ros::Time::now();
odom_to_base.transform.translation.x = 0;
odom_to_base.transform.translation.y = 0;
odom_to_base.transform.translation.z = 0;
odom_to_base.transform.rotation = tf::createQuaternionMsgFromYaw(0);
ros::Time start_time = ros::Time::now();
MouseOdom left_mouse(dev[L], frame[L], cpi);
MouseOdom right_mouse(dev[R], frame[R], cpi);
left_mouse.openDevice();
right_mouse.openDevice();
double x1,y1;
double x2,y2;
double u1,v1;
double u2,v2;
ros::Time stamp1, stamp2;
nav_msgs::Odometry odom;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_link";
odom.pose.pose.position.x = 0.0;
odom.pose.pose.position.y = 0.0;
double th =0, th1 =0, th2 =0;
double dx, dy, dth;
ros::Duration dt;
ros::Time data_stamp = ros::Time::now();
ros::Time prev_data_stamp = data_stamp - ros::Duration(100000000);
double r1=0, r2=0;
double dpm = cpi*39.3700787; //31496.06 : devide counts with this to take meters
while(!g_request_shutdown)
{
left_mouse.readRelativeMove();
right_mouse.readRelativeMove();
left_mouse.getCounts(x1,y1,u1,v1,stamp1);
right_mouse.getCounts(x2,y2,u2,v2,stamp2);
//Stamp data based on latest data from mouses
if (stamp2.toSec() > stamp1.toSec())
data_stamp = stamp2;
else
data_stamp = stamp1;
dt = data_stamp - prev_data_stamp;
prev_data_stamp = data_stamp;
// ROS_INFO("mouse real data stamp: %f", data_stamp.toSec());
// ROS_INFO(" ros::Time data stamp: %f", ros::Time::now().toSec());
//calculation
odom.header.stamp = data_stamp;
dth = atan2( x2-x1 , (0.200*dpm) );
th += dth;
r1 = pow( pow(x1,2) + pow(y1,2) ,0.5 );
r2 = pow( pow(x2,2) + pow(y2,2) ,0.5 );
th1= atan2(y1, x1);
th2= atan2(y2, x2);
dx = ( r1*cos(th+th1) + r2*cos(th+th2) ) / (2*dpm);
dy = ( r1*sin(th+th1) + r2*sin(th+th2) ) / (2*dpm);
odom.pose.pose.position.x += dx;
odom.pose.pose.position.y += dy;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
odom.pose.pose.orientation = odom_quat;
//speed averaging
odom.twist.twist.linear.x = dx/dt.toSec();
odom.twist.twist.linear.y = dy/dt.toSec();
odom.twist.twist.angular.z = dth/dt.toSec();
// ROS_INFO("odom: x %f y %f th %f u %f v %f", odom.pose.pose.position.x, odom.pose.pose.position.y, th, odom.twist.twist.linear.x, odom.twist.twist.linear.y);
odom_pub.publish(odom);
odom_to_base.header.stamp = data_stamp;
odom_to_base.transform.translation.x = odom.pose.pose.position.x;
odom_to_base.transform.translation.y = odom.pose.pose.position.y;
odom_to_base.transform.translation.z = 0.0;
odom_to_base.transform.rotation = odom.pose.pose.orientation;
odom_broadcaster.sendTransform(odom_to_base);
ros::spinOnce();
loop_rate.sleep();
}
if(left_mouse.closeDevice())
ROS_WARN("left mouse couldn't close normally");
if(right_mouse.closeDevice())
ROS_WARN("left mouse couldn't close normally");
ROS_WARN("Mouse devices Closed");
return 0;
}