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;
}
Exemple #3
0
	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;
}