Added StampedTransform to publish multiwii as a valid tf

[emmhaych]

Currently the PoseStamped message is broadcasted by the node. Whilst this gives the correct pose, it is not possible to visualise it using rviz with copter_visualization (part of mavros_extras). This pull request implements two transforms on a base frame which is inputted as a parameter tf_base_frame. The first transformation, multiwii_cartesian against the tf_base_frame sets the altitude of the drone. A second transformation, multiwii against the multiwii_cartesian sets the attitude of the drone.

This pull request in in response to and resolves #2.

[christianrauch]

I get an error message when compiling on ROS kinetic:

[...]/multiwii_node.cpp: In member function ‘void MultiWiiNode::onAltitude(const msp::msg::Altitude&)’:
[...]/multiwii_node.cpp:316:60: error: no matching function for call to ‘tf::Transform::Transform(double, double, double, double)’
         tf::Transform multiwii_transform(0.0, 0.0, 0.0, 0.0);

Some remarks:

• I would like to remove the copter_visualization.rviz since it is quite specific and people probably want to use a different RViz setup
• Can you provide a copter_visualization.launch that is only starting and configuring the copter_visualization?
• The new parameters fixed_frame_id and child_frame_id are not used. They can be removed.
• You can retrieve parameters and set default values via n.param(<param_name>, <variable>, <default_value>) Can you use this instead of the if switch?
• Is it common that the tf::TransformBroadcaster is declared in the publisher function? It somehow appears more reasonable to make the TransformBroadcaster a member of the class and use it in every publisher that wants to publish transforms.

[emmhaych]

Looks like I missed out pushing my last commit.

Added fixes based on your feedback to it too now with the exception of the third bullet. child_frame_id tells copter_visualization what frame to use as the base for the 3d model.

[christianrauch]

Cool. Thanks for your contribution!

Regarding the launch file: I just remember, that I create a separate repo for launch files, that connect the multiwii to other ROS nodes: https://github.com/christianrauch/multiwii_launch. Do you mind to place the copter_visualization launch file there? The idea behind this is, that the launch files in ros-multiwii should only depend on nodes from this package and that launch files from multiwii_launch can depend on any other ROS package.

Apart from this, how does the copter_visualization work? I see poses published on /multiwii/local_position/pose, but no markers published on /vehicle_marker.

Also, I think the transforms are using the wrong frames, i.e. they are mixed up, and some transformation are wrong.

E.g. for the transform with the pure translation part:

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1519480798
        nsecs: 253015426
      frame_id: "map"
    child_frame_id: "multiwii_cartesian"
    transform: 
      translation: 
        x: 0.0
        y: 0.0
        z: 1.5
      rotation: 
        x: 1.0
        y: 0.0
        z: 0.0
        w: 0.0

I think this should be the transform from frame map to multiwii_cartesian. That is, the child frame is map and the transform is reported in the multiwii_cartesian frame. Additionally, the rotation is wrong. In ROS, the quaternion is initialised with Quaternion(x, y, z, w), i.e. the real/scalar part comes last. In the case without rotation, it must be constructed with Quaternion(0, 0, 0, 1).

For the transform with the pure rotation part:

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1519480798
        nsecs: 182768613
      frame_id: "multiwii_cartesian"
    child_frame_id: "multiwii"
    transform: 
      translation: 
        x: 6.94476636698e-310
        y: 6.94474846252e-310
        z: 6.94474846257e-310
      rotation: 
        x: -0.0057175198367
        y: -0.00262854825204
        z: -0.156449419542
        w: 0.987665925213

the frames are also mixed up, i.e. if the rotation transforms from the multiwii_cartesian to multiwii, the child_frame_id should be multiwii_cartesian. Additionally, the translation is not explicitly initialised to 0. The values are random and we should explicitly set them to 0 to prevent random translations applied by this transform. In this specific case, we are just lucky that the random translation is close to 0.

[emmhaych]

1. Moved copter_visualization.launch to christianrauch/multiwii_launch (pull request created)
2. Fixed scaling factor being applied to the correct w axis on multiwii_cartesian tf frame (nice spot!)
3. Initialized translational values to 0 on multiwii tf frame

Regarding your comment that map should be the child of the multiwii_cartesian frame, have a read through tf's frame conventions. In our scenario, map is the base frame (static). It has a child frame multiwii_cartesian with altitude transforms which has a child frame multiwii with attitude transformation. Here's a pictorial view of the frame hierarchy:

Pull request is now updated with your review comments

[christianrauch]

Thanks, you are right about the transforms. I now can see the transform and the marker in RViz.

I haven't looked into the copter_visualization source code and I am still confused about if it uses the published StampedTransform or the PoseStamped. When subscribing to /vehicle_marker, I see parts of the marker published with poses. But I am still unsure where it takes the pose from (from from transformations on /tf or PoseStamped on /multiwii/local_position/pose).

E.g. is one of the sources not required for copter_visualization?

[emmhaych]

I had the same confusion and inspected their source code to find out what's going on. Here is a digest:

• The vehicle_marker marker is only created once when the node is initialised (ref). It uses the child_frame_id parameter to say what the base reference frame is and adds markers with reference to this. There is no pose information used nor are the marker redrawn when pose changes.
• The PoseStamped is instead used to generate track_markers which visualise the flight path taken. (ref)
• RVIZ renders vehicle_marker which are actually just static markers but transforms them using our tf broadcasts (base frame is map and the quad's frame is multiwii

[christianrauch]

Thanks for the clarification.

There is just one last thing: Can you replace the tabs by 4 spaces, such that the blocks align with the rest of the code? :-)

And can you maybe squash all the commits into a single commit that summarises the changes? There are now several commits that and and remove changes again.

[emmhaych]

Replaced tabs with spaces. You should have the option to squash the commits in the merge options dropdown.

[christianrauch]

I know about that option, it just doesn't allow you to change the commit message. But I can do this manually later.

Thanks for this cool feature.