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_); } }
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; }
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); } }
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; }