Closed gy2256 closed 7 years ago
Are you feeding Optitrack data into the Pixhawk through MAVROS?
I am feeding Optitrack Data as /mavros/mocap and I have also changed the Pixhawk parameters such that it uses this information for local position estimation, the result is actually pretty good.
But that being said, I am currently just trying to work with Gazebo simulator (not including mocap). In my code, I create both setpoint_position and setpoint_velcity publishers. I set "set_position.pose.position.z = 2" and "set_velocity.linear.x = 1" But the drone just hover at 2 meters, without moving anywhere.
And it gives me this error:
Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}
I found the issue for the error message. I made a mistake when I try to send '/mavros/setpoint_velocity/cmd_vel'. The message type should be TwistStamped, instead of Twist. After modifying my set_velocity to 'set_velocity.twist.linear.x = 1' the error disappears.
However, a new issue appears. The drone is not stable at all, it keeps shaking as it moves forward (+x direction). I am not sure if my approach is correct. Please let me know if there is a formal way to do it.
Could not process inbound connection: topic types do not match: [geometry_msgs/TwistStamped] vs. [geometry_msgs/Twist]{'topic': '/mavros/setpoint_velocity/cmd_vel', 'tcp_nodelay': '0', 'md5sum': '98d34b0043a2093cf9d9345ab6eef12e', 'type': 'geometry_msgs/TwistStamped', 'callerid': '/mavros'}
Actually the error states what is the problem. The velocity command you are sending is a Twist
msg, while the node is expecting a TwistStamped
msg.
However, a new issue appears. The drone is not stable at all, it keeps shaking as it moves forward (+x direction). I am not sure if my approach is correct. Please let me know if there is a formal way to do it.
Without checking the code you are using, is kinda difficult to understand what's wrong.
@TSC21 Thanks for the help! Unfortunately , I am not sure if I can post my original source code. I will get back to this issue once I have the permission. I apologize for the inconvenience.
I will also post the solution for this particular problem once I find it because I think it is a really good application/demo.
I apologize for the inconvenience.
The inconvenience is not for us that are trying to help you. But asserting over the functionality of a piece of code without knowing the code is kinda difficult.
@TSC21 Yes. I completely understand.
OK. So here is a simple script I have to control both velocity and position.
import rospy
import numpy as np
import math
import mavros_msgs
from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs import srv
from mavros_msgs.msg import State
#=================Parameter Initializaiton========================
goal_pose = PoseStamped()
current_pose = PoseStamped()
set_velocity = TwistStamped()
current_state = State()
def altitude_hold():
global goal_pose
goal_pose.pose.position.z = 2
#==============Call Back Functions=====================
def pos_sub_callback(pose_sub_data):
global current_pose
current_pose = pose_sub_data
def state_callback(state_data):
global current_state
current_state = state_data
#============Intialize Node, Publishers and Subscribers=================
rospy.init_node('Vel_Control_Node', anonymous = True)
rate = rospy.Rate(10) #publish at 10 Hz
local_position_subscribe = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pos_sub_callback)
local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10)
setpoint_velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size = 10)
state_status_subscribe = rospy.Subscriber('/mavros/state',State,state_callback)
altitude_hold()
#============Define Velocity==============================================
set_velocity.twist.linear.x = 1 #moving 1m/s at x direction
while not rospy.is_shutdown():
local_position_pub.publish(goal_pose)
if current_state.mode != "OFFBOARD" or not current_state.armed:
arm = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
arm(True)
set_mode = rospy.ServiceProxy('/mavros/set_mode',mavros_msgs.srv.SetMode)
mode = set_mode(custom_mode = 'OFFBOARD')
if mode.success:
rospy.loginfo('Switched to OFFBOARD mode!')
setpoint_velocity_pub.publish(set_velocity)
rate.sleep()
You should base your code in this: https://dev.px4.io/en/ros/mavros_offboard.html.
I see you send the position setpoint only once while you should send it for a while. Then, when it check it's in the goal_pose
, it should move on x direction with that velocity. Also, you should separate the verification of the mode and arm state into two different conditions.
I am thinking about a different approach. Instead of rely on '/mavros/setpoint_position/local' to achieve the altitude hold, I might as well write my own PID altitude controller that takes the difference of the desired altitude and my current altitude (set_z - measured_z) as my input, and output velocity control in the z direction. Therefore, all I need is to publish Vz to maintain the altitude, it might require some tuning though.
That is something I have already done in test_mavros
with control_toolbox
. Check what is there and maybe you don't have to implement it from scratch.
Thanks! I will take a look at it and I believe it is a great approach. I think this issue can be closed now.
https://github.com/darknight-007/docking/blob/master/testVelocityControl.py
I modified this to get altitude hold while controling horizontal velocity. You can also try setpoint_raw, but I found that it didn't work.
@schmittlema , can you tell me how did you modify to give a constant velocity with altitude hold?
Below. I made a 3 PID controllers for each axis. Then if the desired velocity is none it holds that axis position. Z is always trying to hold altitude. Hopefully it helps. `""" testing offboard positon control with a simple takeoff script """
import rospy from mavros_msgs.msg import State from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped import math import numpy from std_msgs.msg import Header from PID import PID
class VelocityController: target = PoseStamped() output = TwistStamped()
def __init__(self):
self.X = PID()
self.Y = PID()
self.Z = PID()
self.lastTime = rospy.get_time()
self.target = None
def setTarget(self, target):
self.target = target
def update(self, state,x_vel,y_vel,hold_state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
if x_vel == 0:
self.target.position.x = hold_state.pose.position.x
linear.x = self.X.update(self.target.position.x, position.x, time)
else:
linear.x = x_vel
if y_vel == 0:
self.target.position.y = hold_state.pose.position.y
linear.y = self.Y.update(self.target.position.y, position.y, time)
else:
linear.y = y_vel
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
def stop(self):
setTarget(self.current)
update(self.current)`
Hey there. You cannot control Vx, Vy and Z, only velocities or only position. If the drone is shaking it's because it answer to the position setpoint then the velocity.
@AlexisTM currently you can: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L902 https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L1005-L1008 https://github.com/PX4/Firmware/blob/master/src/modules/mc_pos_control/mc_pos_control_main.cpp#L1612
Bitmasks are:
to control vx, vy, z, y -> 3043
(0xBE3
)
to control vx, vy, z -> 4067
(0xFE3
)
to control vx, vy, z, y_rate -> 2019
(0x7E3
)
Indeed, but he mentioned using setpoint_position
and setpoint_velocity
. He has to use setpoint_raw
and set the type_mask.
Also, I am not certain the mode is working as the offboard_mode
is converted to a vehicule_mode
and the offboard_mode.ignore_alt_hold
is not mentioned at all. Therefore, I never tested it.
I am a PhD student currently working on an experiment that requires me to control drones with 2D Velocity Command (Vx, Vy). Essentially I want the drone to hold at a constant altitude and move around on the x,y plane.
My current setup is the following: DJI F450 frame, Odroid XU4, Pixhawk, PX4 Firmware with LPE (I am also using Optitrack). At this point, I am just trying to get my algorithm working in the Gazebo Simulator. So far, I have got the waypoints working but I am having trouble to let the drone hold its position while sending Vx, Vy. Do anyone have experience with this? Any help would be appreciated!