search

thien94 / vision_to_mavros

A collection of ROS and non-ROS (Python) code that converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding mavros topics or MAVLink messages that can be consumed by a flight control stack (with working and tested examples for ArduPilot).
https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html
GNU General Public License v3.0
242 stars 146 forks source link

Is it possible to use vision_to_mavros repo for usb_cam_node? #6

Closed hughhugh closed 5 years ago

hughhugh commented 5 years ago

Hi, @hoangthien94 I am using a Logitech camera to identify the ARUCO and estimate the pose. The problem is I do not have a T265 with me right now. So is the vision_to_mavros repo compatible with the usb_cam_node? I can get the image and recognize the pose. Can I use it to transform the cam_pose by this node? Thank you.

thien94 commented 5 years ago

Hi @hughhugh,

The ROS nodes that are responsible for camera driver (usb_cam) and fiducial tag detection (Aruco) are not part of the package, as the vision_to_mavros node only reads from /tf topic, processes data, then re-publishes a new topic that is compatible with mavros. In other words, the choice of camera and tag is independent of this node, as long as the data contained in /tf is correct.

Specifically for the case of Aruco tag, there is already an ArduPilot wiki page and blog post that explains in details what need to be done, so check them out.

hughhugh commented 5 years ago

@hoangthien94 I have tried the April tag today. I can rostopic echo /tf， but I can’t echo the /mavros/vision_pose/pose. I have put the tag board in front of the camera. Thanks. 

  ROS_INFO("Error happens .................................");
  ROS_INFO("The source_frame_id parameter: %s", source_frame_id.c_str());
  ROS_INFO("The target_frame_id parameter: %s", target_frame_id.c_str());
  ROS_INFO("The output_rate parameter: %f", output_rate);
  // lookupTransform(frame_2, frame_1, at_this_time, this_transform)
  // will give the transfrom from frame_1 to frame_2
  tf_listener.lookupTransform(target_frame_id, source_frame_id, now, transform);

The terminal shows that before I put a tag board in front of the camera

[ INFO] [1568684872.516637457]: Error happens ................................. [ INFO] [1568684872.516696576]: The source_frame_id parameter: camera [ INFO] [1568684872.516721281]: The target_frame_id parameter: A3_bundle [ INFO] [1568684872.516748337]: The output_rate parameter: 30.000000 [ WARN] [1568684872.516813127]: "A3_bundle" passed to lookupTransform argument target_frame does not exist.

after I put a tag board

[ INFO] [1568684876.517944030]: Error happens ................................. [ INFO] [1568684876.518024658]: The source_frame_id parameter: camera [ INFO] [1568684876.518068295]: The target_frame_id parameter: A3_bundle [ INFO] [1568684876.518124780]: The output_rate parameter: 30.000000 [ WARN] [1568684876.518210863]: "camera" passed to lookupTransform argument source_frame does not exist.

thien94 commented 5 years ago

May I suggest that you echo the /tf topic and figure out the source_frame_id and target_frame_id from your aruco tag first, replace the respective params in the launch file or in the code, then try again and see how it goes.

hughhugh commented 5 years ago

@hoangthien94 Hi, I am using Apiltag to test the vision_to_mavros according to the Ardupilot Apriltag wiki. And I get this below.

[ WARN] [1568684856.488308524]: remove_duplicates parameter not provided. Defaulting to true
[ INFO] [1568684856.494312031]: Get source_frame_id parameter: camera
[ INFO] [1568684856.495976301]: Get output_rate parameter: 30.000000
[ INFO] [1568684856.496842417]: Get gamma_world parameter: -1.570796
[ INFO] [1568684856.498149188]: Get roll_cam parameter: 0.000000
[ INFO] [1568684856.499348769]: Get pitch_cam parameter: 0.000000
[ INFO] [1568684856.500094645]: Get yaw_cam parameter: 0.000000
[ INFO] [1568684856.500151475]: The source_frame_id parameter: camera
[ INFO] [1568684856.500175728]: The target_frame_id parameter: A3_bundle
[ INFO] [1568684856.500205141]: The output_rate parameter: 30.000000
[ INFO] [1568684856.524789980]: Using transport "raw"
[ INFO] [1568684856.601138879]: camera calibration URL: file:///home/wuhugh/ardusub/ost.yaml
[ INFO] [1568684856.605649951]: Starting 'camera' (/dev/video0) at 640x480 via mmap (yuyv) at 30 FPS

I modify the launch according to your suggestions. It works now. The /tf source id is camera_frame not camera. I have another question, that is if I use usb_cam not T265, does the rviz work to see the axis? Thank you.

thien94 commented 5 years ago

Glad it works out for you. The RViz should work as described in the wiki.

hughhugh commented 5 years ago

@hoangthien94 I have read the src code, it seems that the position of the camera just equals to the position_orig if the camera aligns to the world frame. The QuaternionFromRPY is first along thex axis, second they axis, finally rotate along the z axis. The orientation of the camera is saved in thequat_body. In my case, I combine the apriltag with ardurover firmware. Maybe I have to modify the src code. Thank you.