troiwill / mavros-px4-vehicle

A Python API developed to control a PX4-enabled drone with MAVROS commands.
GNU General Public License v2.0
7 stars 2 forks source link

SetRawCmdBuilder doesn't works #2

Open Ultramarine1939-syujie opened 1 year ago

Ultramarine1939-syujie commented 1 year ago

when I want to change message and let thr drone go where I ordered, one error occured:

Exception in thread Thread-6:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.8/threading.py", line 870, in run
    self._target(*self._args, **self._kwargs)
  File "/home/ubuntu/catkin_ws/src/mavros-px4-vehicle/src/mavros_px4_vehicle/px4_offboard_pub.py", line 75, in __publish_loop__
    publisher.publish(data)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/topics.py", line 879, in publish
    data = args_kwds_to_message(self.data_class, args, kwds)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/msg.py", line 121, in args_kwds_to_message
    raise TypeError("expected [%s] but got [%s]"%(data_class._slot_types[0], arg._type))
TypeError: expected [geometry_msgs/Vector3] but got [mavros_msgs/PositionTarget]

and here's the program I've written:

#!/usr/bin/env python

import math
import rospy
from mavros_px4_vehicle.px4_modes import PX4_MODE_LOITER,PX4_MODE_OFFBOARD,PX4_MODE_TAKEOFF,PX4_MODE_LAND
from mavros_px4_vehicle.px4_offboard_modes import SetRawCmdBuilder
from mavros_px4_vehicle.px4_vehicle import PX4Vehicle

def offboard_test():
    # Create an instance and arm the drone.
    rospy.loginfo("Connecting to the drone.")
    drone = PX4Vehicle(auto_connect = True)
    drone.arm()
    drone.wait_for_status(drone.is_armed, True, 2)

    drone.takeoff(block=True)

    zero_cmd = SetRawCmdBuilder.build()
    fwd_cmd = SetRawCmdBuilder.build(vx = 1)
    right_turn_cmd = SetRawCmdBuilder.build(yawrate = math.radians(15.))

    rospy.sleep(2.)
    rospy.loginfo("Changing to offboard mode.")
    drone.set_mode(PX4_MODE_OFFBOARD)

    count = 0
    while count < 4:
        rospy.loginfo("Making the drone fly forward.")
        drone.set_posvelacc(fwd_cmd)
        rospy.sleep(5.)

        rospy.loginfo("Making the drone turn right #{}.\n".format(count + 1))
        drone.set_velocity(zero_cmd)
        rospy.sleep(2.)
        drone.set_posvelacc(right_turn_cmd)
        rospy.sleep(6.)
        count = count + 1

    rospy.sleep(2.)
    rospy.loginfo("Changing to loiter mode.")
    drone.set_mode(PX4_MODE_LOITER)
    rospy.sleep(4.)

    rospy.loginfo("Making the drone land.")
    drone.land(block=True)

    rospy.spin()

if __name__ == "__main__":
    try:
        rospy.init_node("offboard_fly_test")
        offboard_test()

    except:
        pass
troiwill commented 1 year ago

Hi @Ultramarine1939-syujie.

Sorry for the late reply, and thank you for your interest in the project! If I remember correctly, sending MAVROS setpoint_raw local commands in Python was an issue when I first wrote this code. (SetRawCmdBuilder creates commands for setpoint_raw local). Therefore, I only use SetPositionWithYawCmdBuilder for now.

If SetPositionWithYawCmdBuilder can fulfill your needs, I suggest using it. Otherwise, unfortunately, I do not have another solution right now.