Global planner yaw goal about px4-avoidance HOT 12 CLOSED

Just a heads up - we figured out how to do this. It's very simple and I feel silly for not noticing it before. Just publish a PoseWithCovariance to the /initialpose topic. You can set your yaw in this, and the vehicle will travel to this pose, without obstacle avoidance of course

from px4-avoidance.

from px4-avoidance.

@tschmidt64-SC right now you cannot specify the vehicle heading at the goal. This is where the message gets handled:
https://github.com/PX4/avoidance/blob/606c5a71f79b9c27d40ffc702713081ec8cfc163/global_planner/src/nodes/global_planner_node.cpp#L259-L260

from px4-avoidance.

Would yall accept a PR for this feature?

from px4-avoidance.

@tschmidt64-SC The reason why we have chosen not to specify a heading at a waypoint is the following. Let's assume that because of the obstacle geometry the vehicle gets to a waypoint with a heading different from the one required. What the vehicle should do? Yaw until it has reached the desired yaw at the waypoint and then proceed to the next waypoint? it seems a bit unnatural.

However, if you have a particular use case for this feature I am open to consider it :)

from px4-avoidance.

@tschmidt64-SC ok, I see.

In offboard all manual controls are ignored. You would need to change the FlightTask Offboard on the autopilot Firmware side to achieve what you're describing.

We could could have the goal orientation as optional and have the default behavior as it is now. Of course, your contribution is more than welcomed!

from px4-avoidance.

Can you clarify what you mean/are referring to by, "You would need to change the FlightTask Offboard on the autopilot Firmware side"?

To be more clear, we're not issuing manual controls (like from a remote). We are using offboard commands as well by publishing to the setpoint position topics etc. The issue for us is that so is the Avoidance code, and having them both publishing at the same time doesn't work.

from px4-avoidance.

@tschmidt64-SC I misunderstood

we tried to manually yaw

I thought you were trying to control the yaw with the sticks on the remote control.

from px4-avoidance.

Just a heads up - we figured out how to do this. It's very simple and I feel silly for not noticing it before. Just publish a PoseWithCovariance to the /initialpose topic. You can set your yaw in this, and the vehicle will travel to this pose, without obstacle avoidance of course

Hey tschmidt64-SC, I'm trying also to specify the yaw angle at given waypoints from a python script running in conjunction to the global_planner/local_planner. I tried PoseWithCovariance to the /initialpose topic, but it gave me some error that I should use PoseWithCovarianceStamped instead. Is this how it was for you?

Unfortunately, PoseWithCovarianceStamped didn't work for me either.

Did you set the publisher as something like:

"yaw_pub = rospy.Publisher('/initialpose',PoseWithCovariance, queue_size=10)"?

And then publish with something like:

	"yaw_pub.publish(K.rotation)"?

from px4-avoidance.

Here is the error I get.

Could not process inbound connection: topic types do not match: [geometry_msgs/PoseWithCovarianceStamped] vs. [geometry_msgs/PoseWithCovariance]{'topic': '/initialpose', 'tcp_nodelay': '0', 'md5sum': '953b798c0f514ff060a53a3498ce6246', 'type': 'geometry_msgs/PoseWithCovarianceStamped', 'callerid': '/path_handler_node'}
[WARN] [1544619893.747193, 856.912000]: Could not process inbound connection: topic types do not match: [geometry_msgs/PoseWithCovarianceStamped] vs. [geometry_msgs/PoseWithCovariance]{'message_definition': "# This expresses an estimated pose with a reference coordinate frame and timestamp\n\nHeader header\nPoseWithCovariance pose\n\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data \n# in a particular coordinate frame.\n# \n# sequence ID: consecutively increasing ID \nuint32 seq\n#Two-integer timestamp that is expressed as:\n# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n# time-handling sugar is provided by the client library\ntime stamp\n#Frame this data is associated with\n# 0: no frame\n# 1: global frame\nstring frame_id\n\n================================================================================\nMSG: geometry_msgs/PoseWithCovariance\n# This represents a pose in free space with uncertainty.\n\nPose pose\n\n# Row-major representation of the 6x6 covariance matrix\n# The orientation parameters use a fixed-axis representation.\n# In order, the parameters are:\n# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\nfloat64[36] covariance\n\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation. \nPoint position\nQuaternion orientation\n\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x\nfloat64 y\nfloat64 z\nfloat64 w\n", 'callerid': '/rostopic_17083_1544619828239', 'tcp_nodelay': '0', 'md5sum': '953b798c0f514ff060a53a3498ce6246', 'topic': '/initialpose', 'type': 'geometry_msgs/PoseWithCovarianceStamped'}

from px4-avoidance.

Yes sorry typo on my part. You should create a PoseWithCovarianceStamped object with the values you want and publish that to the /initialpose topic

You can’t just publish a rotation (not sure what the type of that object is), you have to create a PoseWithCovarianceStamped object and publish that specifically

from px4-avoidance.

Yes sorry typo on my part. You should create a PoseWithCovarianceStamped object with the values you want and publish that to the /initialpose topic

You can’t just publish a rotation (not sure what the type of that object is), you have to create a PoseWithCovarianceStamped object and publish that specifically

Yes, rotation is a variable set to PoseWithCovarianceStamped() so it actually includes everything in the PoseWithCovarianceStamped object.

Right now, the drone in the global_planner_sitl_mavros.launch simulation goes to the desired location but still does not rotate (I've used the tf.transformation.quaternion_from_euler() function to set the desired yaw in radians and convert that to quaternion for publishing.)

Overall, the local_planner behaves way nicer in simulation than the global_planner simulation I'm trying to run. Any ideas if the yaw is possible using local_planner instead?

from px4-avoidance.