SetRawCmdBuilder doesn't works #2
Description
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