forked from cesar.alejandro/oscillation_ctrl
Added service to set waypoints
This commit is contained in:
parent
0c0764387b
commit
4fbed7a57c
|
|
@ -0,0 +1,270 @@
|
|||
# Common configuration for PX4 autopilot
|
||||
#
|
||||
# node:
|
||||
startup_px4_usb_quirk: true
|
||||
|
||||
# --- system plugins ---
|
||||
|
||||
# sys_status & sys_time connection options
|
||||
conn:
|
||||
heartbeat_rate: 1.0 # send hertbeat rate in Hertz
|
||||
timeout: 10.0 # hertbeat timeout in seconds
|
||||
timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0)
|
||||
system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0)
|
||||
|
||||
# sys_status
|
||||
sys:
|
||||
min_voltage: 10.0 # diagnostics min voltage
|
||||
disable_diag: false # disable all sys_status diagnostics, except heartbeat
|
||||
|
||||
# sys_time
|
||||
time:
|
||||
time_ref_source: "fcu" # time_reference source
|
||||
timesync_mode: MAVLINK
|
||||
timesync_avg_alpha: 0.6 # timesync averaging factor
|
||||
|
||||
# --- mavros plugins (alphabetical order) ---
|
||||
|
||||
# 3dr_radio
|
||||
tdr_radio:
|
||||
low_rssi: 40 # raw rssi lower level for diagnostics
|
||||
|
||||
# actuator_control
|
||||
# None
|
||||
|
||||
# command
|
||||
cmd:
|
||||
use_comp_id_system_control: false # quirk for some old FCUs
|
||||
|
||||
# dummy
|
||||
# None
|
||||
|
||||
# ftp
|
||||
# None
|
||||
|
||||
# global_position
|
||||
global_position:
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
rot_covariance: 99999.0 # covariance for attitude?
|
||||
gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m)
|
||||
use_relative_alt: true # use relative altitude for local coordinates
|
||||
tf:
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
global_frame_id: "earth" # TF earth frame_id
|
||||
child_frame_id: "base_link" # TF child_frame_id
|
||||
|
||||
# imu_pub
|
||||
imu:
|
||||
frame_id: "base_link"
|
||||
# need find actual values
|
||||
linear_acceleration_stdev: 0.0003
|
||||
angular_velocity_stdev: 0.0003490659 // 0.02 degrees
|
||||
orientation_stdev: 1.0
|
||||
magnetic_stdev: 0.0
|
||||
|
||||
# local_position
|
||||
local_position:
|
||||
frame_id: "map"
|
||||
tf:
|
||||
send: false
|
||||
frame_id: "map"
|
||||
child_frame_id: "base_link"
|
||||
send_fcu: false
|
||||
|
||||
# param
|
||||
# None, used for FCU params
|
||||
|
||||
# rc_io
|
||||
# None
|
||||
|
||||
# safety_area
|
||||
safety_area:
|
||||
p1: {x: 1.0, y: 1.0, z: 1.0}
|
||||
p2: {x: -1.0, y: -1.0, z: -1.0}
|
||||
|
||||
# setpoint_accel
|
||||
setpoint_accel:
|
||||
send_force: false
|
||||
|
||||
# setpoint_attitude
|
||||
setpoint_attitude:
|
||||
reverse_thrust: false # allow reversed thrust
|
||||
use_quaternion: true # enable PoseStamped topic subscriber
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_attitude"
|
||||
rate_limit: 50.0
|
||||
|
||||
setpoint_raw:
|
||||
thrust_scaling: 1.0 # used in setpoint_raw attitude callback.
|
||||
# Note: PX4 expects normalized thrust values between 0 and 1, which means that
|
||||
# the scaling needs to be unitary and the inputs should be 0..1 as well.
|
||||
|
||||
# setpoint_position
|
||||
setpoint_position:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "target_position"
|
||||
rate_limit: 50.0
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# setpoint_velocity
|
||||
setpoint_velocity:
|
||||
mav_frame: LOCAL_NED
|
||||
|
||||
# vfr_hud
|
||||
# None
|
||||
|
||||
# waypoint
|
||||
mission:
|
||||
pull_after_gcs: true # update mission if gcs updates
|
||||
|
||||
# --- mavros extras plugins (same order) ---
|
||||
|
||||
# adsb
|
||||
# None
|
||||
|
||||
# debug_value
|
||||
# None
|
||||
|
||||
# distance_sensor
|
||||
## Currently available orientations:
|
||||
# Check http://wiki.ros.org/mavros/Enumerations
|
||||
##
|
||||
distance_sensor:
|
||||
hrlv_ez4_pub:
|
||||
id: 0
|
||||
frame_id: "hrlv_ez4_sonar"
|
||||
orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
lidarlite_pub:
|
||||
id: 1
|
||||
frame_id: "lidarlite_laser"
|
||||
orientation: PITCH_270
|
||||
field_of_view: 0.0 # XXX TODO
|
||||
send_tf: true
|
||||
sensor_position: {x: 0.0, y: 0.0, z: -0.1}
|
||||
sonar_1_sub:
|
||||
subscriber: true
|
||||
id: 2
|
||||
orientation: PITCH_270
|
||||
laser_1_sub:
|
||||
subscriber: true
|
||||
id: 3
|
||||
orientation: PITCH_270
|
||||
|
||||
# image_pub
|
||||
image:
|
||||
frame_id: "px4flow"
|
||||
|
||||
# fake_gps
|
||||
fake_gps:
|
||||
# select data source
|
||||
use_mocap: true # ~mocap/pose
|
||||
mocap_transform: false # ~mocap/tf instead of pose
|
||||
use_vision: false # ~vision (pose)
|
||||
# origin (default: Zürich)
|
||||
geo_origin:
|
||||
lat: 47.3667 # latitude [degrees]
|
||||
lon: 8.5500 # longitude [degrees]
|
||||
alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters]
|
||||
eph: 2.0
|
||||
epv: 2.0
|
||||
satellites_visible: 5 # virtual number of visible satellites
|
||||
fix_type: 3 # type of GPS fix (default: 3D)
|
||||
tf:
|
||||
listen: false
|
||||
send: false # send TF?
|
||||
frame_id: "map" # TF frame_id
|
||||
child_frame_id: "fix" # TF child_frame_id
|
||||
rate_limit: 10.0 # TF rate
|
||||
gps_rate: 5.0 # GPS data publishing rate
|
||||
|
||||
# landing_target
|
||||
landing_target:
|
||||
listen_lt: false
|
||||
mav_frame: "LOCAL_NED"
|
||||
land_target_type: "VISION_FIDUCIAL"
|
||||
image:
|
||||
width: 640 # [pixels]
|
||||
height: 480
|
||||
camera:
|
||||
fov_x: 2.0071286398 # default: 115 [degrees]
|
||||
fov_y: 2.0071286398
|
||||
tf:
|
||||
send: true
|
||||
listen: false
|
||||
frame_id: "landing_target"
|
||||
child_frame_id: "camera_center"
|
||||
rate_limit: 10.0
|
||||
target_size: {x: 0.3, y: 0.3}
|
||||
|
||||
# mocap_pose_estimate
|
||||
mocap:
|
||||
# select mocap source
|
||||
use_tf: false # ~mocap/tf
|
||||
use_pose: true # ~mocap/pose
|
||||
|
||||
# odom
|
||||
odometry:
|
||||
in:
|
||||
frame_id: "odom"
|
||||
child_frame_id: "base_link"
|
||||
frame_tf:
|
||||
local_frame: "local_origin_ned"
|
||||
body_frame_orientation: "flu"
|
||||
out:
|
||||
frame_tf:
|
||||
# available: check MAV_FRAME odometry local frames in
|
||||
# https://mavlink.io/en/messages/common.html
|
||||
local_frame: "vision_ned"
|
||||
# available: ned, frd or flu (though only the tf to frd is supported)
|
||||
body_frame_orientation: "frd"
|
||||
|
||||
# px4flow
|
||||
px4flow:
|
||||
frame_id: "px4flow"
|
||||
ranger_fov: 0.118682 # 6.8 degrees at 5 meters, 31 degrees at 1 meter
|
||||
ranger_min_range: 0.3 # meters
|
||||
ranger_max_range: 5.0 # meters
|
||||
|
||||
# vision_pose_estimate
|
||||
vision_pose:
|
||||
tf:
|
||||
listen: false # enable tf listener (disable topic subscribers)
|
||||
frame_id: "map"
|
||||
child_frame_id: "vision_estimate"
|
||||
rate_limit: 10.0
|
||||
|
||||
# vision_speed_estimate
|
||||
vision_speed:
|
||||
listen_twist: true # enable listen to twist topic, else listen to vec3d topic
|
||||
twist_cov: true # enable listen to twist with covariance topic
|
||||
|
||||
# vibration
|
||||
vibration:
|
||||
frame_id: "base_link"
|
||||
|
||||
# wheel_odometry
|
||||
wheel_odometry:
|
||||
count: 2 # number of wheels to compute odometry
|
||||
use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry
|
||||
wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m)
|
||||
send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance)
|
||||
send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry
|
||||
frame_id: "map" # origin frame
|
||||
child_frame_id: "base_link" # body-fixed frame
|
||||
vel_error: 0.1 # wheel velocity measurement error 1-std (m/s)
|
||||
tf:
|
||||
send: true
|
||||
frame_id: "map"
|
||||
child_frame_id: "base_link"
|
||||
|
||||
# vim:set ts=2 sw=2 et:
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
plugin_blacklist:
|
||||
# common
|
||||
- safety_area
|
||||
# extras
|
||||
- image_pub
|
||||
- vibration
|
||||
- distance_sensor
|
||||
- rangefinder
|
||||
- wheel_odometry
|
||||
- mission
|
||||
- tdr_radio
|
||||
- fake_gps
|
||||
|
||||
plugin_whitelist: []
|
||||
#- 'sys_*'
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
digraph G {
|
||||
"local_origin" -> "local_origin_ned"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
|
||||
"fcu" -> "fcu_frd"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
|
||||
"fcu_frd" -> "fcu"[label="Broadcaster: /mavros\nAverage rate: 10000.000 Hz\nMost recent transform: 0.000 ( 124.428 sec old)\nBuffer length: 0.000 sec\n"];
|
||||
edge [style=invis];
|
||||
subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
|
||||
"Recorded at time: 124.428"[ shape=plaintext ] ;
|
||||
}->"local_origin";
|
||||
}
|
||||
BIN
frames.pdf
BIN
frames.pdf
Binary file not shown.
|
|
@ -14,6 +14,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
pkg="oscillation_ctrl"
|
||||
type="MoCap_Localization_noTether.py"
|
||||
name="localize_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
||||
<!-- Cortex bridge launch -->
|
||||
|
|
@ -21,4 +22,6 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
|
||||
<!-- MAVROS launch -->
|
||||
<include file="$(find mavros)/launch/px4.launch"/>
|
||||
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -22,6 +22,11 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
name="linkStates_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="ref_signalGen.py"
|
||||
|
|
|
|||
|
|
@ -11,7 +11,6 @@ from tf.transformations import *
|
|||
from offboard_ex.msg import tethered_status
|
||||
from geometry_msgs.msg import Pose
|
||||
from gazebo_msgs.srv import GetLinkState
|
||||
from oscillation_ctrl.srv import WaypointTrack
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
|
|
@ -53,6 +52,9 @@ class Main:
|
|||
self.phidot_max = 3.0
|
||||
self.thetadot_max = 3.0
|
||||
|
||||
# Vehicle is spawned with yaw offset for convenience, need to deal with that
|
||||
self.yaw_offset = 0.0
|
||||
|
||||
# variables for message gen
|
||||
self.status = tethered_status()
|
||||
self.status.drone_id = 'spiri_with_tether::spiri::base'
|
||||
|
|
@ -97,18 +99,18 @@ class Main:
|
|||
output = value
|
||||
return output
|
||||
|
||||
def euler_array(self,pose):
|
||||
def euler_array(self,orientation):
|
||||
"""
|
||||
Takes in pose msg object and outputs array of euler angs:
|
||||
q[0] = Roll
|
||||
q[1] = Pitch
|
||||
q[2] = Yaw
|
||||
eul[0] = Roll
|
||||
eul[1] = Pitch
|
||||
eul[2] = Yaw
|
||||
"""
|
||||
q = euler_from_quaternion([pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w])
|
||||
return q
|
||||
eul = euler_from_quaternion([orientation.x,
|
||||
orientation.y,
|
||||
orientation.z,
|
||||
orientation.w])
|
||||
return eul
|
||||
|
||||
# Get link states (drone and pload) and determine angle between them
|
||||
def link_state(self,pub_timer):
|
||||
|
|
@ -125,9 +127,9 @@ class Main:
|
|||
# P = Position vector
|
||||
drone_P = get_P(self.status.drone_id,reference)
|
||||
|
||||
# Get orientation of drone in euler angles
|
||||
drone_Eul = self.euler_array(drone_P.link_state.pose)
|
||||
|
||||
# Get orientation of drone in euler angles to determine yaw offset
|
||||
drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
|
||||
|
||||
# Check if payload is part of simulation
|
||||
if not drone_P.success:
|
||||
self.status.drone_id = 'spiri::base'
|
||||
|
|
@ -138,6 +140,9 @@ class Main:
|
|||
|
||||
if not self.has_run == 1:
|
||||
if self.pload == True:
|
||||
# Determine yaw offset
|
||||
self.yaw_offset = drone_Eul[2]
|
||||
|
||||
# Get tether length based off initial displacement
|
||||
self.tetherL = math.sqrt((drone_P.link_state.pose.position.x -
|
||||
pload_P.link_state.pose.position.x)**2 +
|
||||
|
|
@ -158,6 +163,19 @@ class Main:
|
|||
drone_Py = drone_P.link_state.pose.position.y
|
||||
drone_Pz = drone_P.link_state.pose.position.z
|
||||
|
||||
# Get drone orientation
|
||||
drone_q = [drone_P.link_state.pose.orientation.x,drone_P.link_state.pose.orientation.y,
|
||||
drone_P.link_state.pose.orientation.z,drone_P.link_state.pose.orientation.w]
|
||||
|
||||
# offset orientation by yaw offset
|
||||
q_offset = quaternion_from_euler(0,0,-self.yaw_offset)
|
||||
#drone_q = quaternion_multiply(drone_q,q_offset)
|
||||
|
||||
drone_P.link_state.pose.orientation.x = drone_q[0]
|
||||
drone_P.link_state.pose.orientation.y = drone_q[1]
|
||||
drone_P.link_state.pose.orientation.z = drone_q[2]
|
||||
drone_P.link_state.pose.orientation.w = drone_q[3]
|
||||
|
||||
if self.pload == True: # If there is payload, determine the variables
|
||||
# Pload
|
||||
pload_Px = pload_P.link_state.pose.position.x
|
||||
|
|
@ -195,7 +213,7 @@ class Main:
|
|||
# Print and save results
|
||||
print "\n"
|
||||
rospy.loginfo("")
|
||||
print"Roll: "+str(round(drone_Eul[0],2)),"\nPitch: "+str(round(drone_Eul[1],2)),"\nYaw: "+str(round(drone_Eul[2],2))
|
||||
print"Roll: "+str(round(drone_Eul[0],2)),"\nPitch: "+str(round(drone_Eul[1],2)),"\nYaw: "+str(round(drone_Eul[2],4))
|
||||
print "drone pos.x: " + str(round(drone_Px,2))
|
||||
print "drone pos.y: " + str(round(drone_Py,2))
|
||||
print "drone pos.z: " + str(round(drone_Pz,2))
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ import time
|
|||
import math
|
||||
from tf.transformations import *
|
||||
from oscillation_ctrl.msg import tethered_status
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from geometry_msgs.msg import PoseStamped, Point
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
|
|
@ -19,8 +19,6 @@ class Main:
|
|||
# rate(s)
|
||||
rate = 40 # rate for the publisher method, specified in Hz -- 20 Hz
|
||||
|
||||
self.dt = 1.0/rate
|
||||
|
||||
# Variables needed for testing start
|
||||
self.tstart = rospy.get_time() # Keep track of the start time
|
||||
while self.tstart == 0.0: # Need to make sure get_rostime works
|
||||
|
|
@ -41,19 +39,10 @@ class Main:
|
|||
### -*-*-*- END -*-*-*- ###
|
||||
|
||||
# initialize variables
|
||||
#self.buff_pose1 = PoseStamped()
|
||||
self.drone_pose = PoseStamped()
|
||||
self.buff_pose = PoseStamped()
|
||||
|
||||
# Debugging
|
||||
# self.buff_pose.pose.position.y = 0
|
||||
# self.buff_pose.pose.position.x = 0
|
||||
# self.buff_pose.pose.position.z = 0
|
||||
# self.buff_pose.pose.orientation.y = 0
|
||||
# self.buff_pose.pose.orientation.x = 0
|
||||
# self.buff_pose.pose.orientation.z = 0
|
||||
# self.buff_pose.pose.orientation.w = 1
|
||||
# self.buff_pose.header.frame_id = 'map'
|
||||
self.eul = [0.0,0.0,0.0]
|
||||
|
||||
# Max dot values to prevent 'blowup'
|
||||
self.phidot_max = 3.0
|
||||
|
|
@ -71,12 +60,6 @@ class Main:
|
|||
|
||||
|
||||
# publisher(s)
|
||||
#self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
|
||||
#self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
|
||||
|
||||
#self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
|
||||
#self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
|
||||
|
||||
### Since there is no tether, we can publish directly to mavros
|
||||
self.pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
|
||||
|
||||
|
|
@ -104,18 +87,17 @@ class Main:
|
|||
output = value
|
||||
return output
|
||||
|
||||
def euler_array(self,pose):
|
||||
def euler_array(self):
|
||||
"""
|
||||
Takes in pose msg object and outputs array of euler angs:
|
||||
q[0] = Roll
|
||||
q[1] = Pitch
|
||||
q[2] = Yaw
|
||||
eul[0] = Roll
|
||||
eul[1] = Pitch
|
||||
eul[2] = Yaw
|
||||
"""
|
||||
q = euler_from_quaternion([pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w])
|
||||
return q
|
||||
self.eul = euler_from_quaternion([self.drone_pose.pose.orientation.x,
|
||||
self.drone_pose.pose.orientation.y,
|
||||
self.drone_pose.pose.orientation.z,
|
||||
self.drone_pose.pose.orientation.w])
|
||||
|
||||
def FRD_Transform(self):
|
||||
'''
|
||||
|
|
@ -123,7 +105,20 @@ class Main:
|
|||
'''
|
||||
|
||||
self.drone_pose = self.buff_pose
|
||||
self.drone_pose.header.frame_id = "map"
|
||||
self.drone_pose.header.frame_id = "/map"
|
||||
|
||||
# self.drone_pose.pose.position.x = 0
|
||||
# self.drone_pose.pose.position.y = 0
|
||||
# self.drone_pose.pose.position.z = 0.5
|
||||
|
||||
#Keep the w same and change x, y, and z as above.
|
||||
# self.drone_pose.pose.orientation.x = 0
|
||||
# self.drone_pose.pose.orientation.y = 0
|
||||
# self.drone_pose.pose.orientation.z = 0
|
||||
# self.drone_pose.pose.orientation.w = 1
|
||||
|
||||
self.euler_array() # get euler angles of orientation for user
|
||||
|
||||
# self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
|
||||
# self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
|
||||
# self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
|
||||
|
|
@ -157,6 +152,9 @@ class Main:
|
|||
print "drone pos.x: " + str(round(self.drone_pose.pose.position.x,2))
|
||||
print "drone pos.y: " + str(round(self.drone_pose.pose.position.y,2))
|
||||
print "drone pos.z: " + str(round(self.drone_pose.pose.position.z,2))
|
||||
print "Roll: " + str(round(self.eul[0]*180/3.14,2))
|
||||
print "Pitch: " + str(round(self.eul[1]*180/3.14,2))
|
||||
print "Yaw: " + str(round(self.eul[2]*180/3.14,2))
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
|
|
|
|||
|
|
@ -308,9 +308,6 @@ class Main:
|
|||
|
||||
# Attitude control
|
||||
self.quaternion.header.stamp = rospy.Time.now()
|
||||
# self.quaternion.pose.position.x = 0
|
||||
# self.quaternion.pose.position.y = 0
|
||||
# self.quaternion.pose.position.z = 5.0
|
||||
self.quaternion.pose.orientation.x = q[0]
|
||||
self.quaternion.pose.orientation.y = q[1]
|
||||
self.quaternion.pose.orientation.z = q[2]
|
||||
|
|
|
|||
|
|
@ -2,7 +2,6 @@
|
|||
* @file offb_node.cpp
|
||||
* @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
|
||||
* Stack and tested in Gazebo SITL
|
||||
* Cesar: Now used for path following on topic \path
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
|
|
|||
|
|
@ -5,8 +5,9 @@
|
|||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <offboard_ex/RefSignal.h>
|
||||
#include <offboard_ex/EulerAngles.h>
|
||||
#include <oscillation_ctrl/RefSignal.h>
|
||||
#include <oscillation_ctrl/EulerAngles.h>
|
||||
#include <oscillation_ctrl/WaypointTrack.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
|
|
@ -31,7 +32,7 @@ geometry_msgs::PoseStamped desPose;
|
|||
tf2::Quaternion q_cmd /*What I send*/, q_Jae; // What should be sent;
|
||||
double rol_K, pch_K, yaw_K, rol_J, pch_J, yaw_J; // Same logic, K = cmd
|
||||
geometry_msgs::Vector3 K_ang, J_ang; // Needed to populate EulerAngle msg
|
||||
offboard_ex::EulerAngles eulAng;
|
||||
oscillation_ctrl::EulerAngles eulAng;
|
||||
|
||||
mavros_msgs::AttitudeTarget thrust;
|
||||
|
||||
|
|
@ -42,12 +43,9 @@ void state_cb(const mavros_msgs::State::ConstPtr& msg){
|
|||
}
|
||||
|
||||
geometry_msgs::Quaternion q_des;
|
||||
double alt_des; // Check desired height
|
||||
|
||||
// Cb to determine desired pose *Only needed for orientation
|
||||
void q_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
||||
desPose = *msg;
|
||||
alt_des = desPose.pose.position.z;
|
||||
q_des = desPose.pose.orientation;
|
||||
tf2::convert(q_des,q_cmd); // Convert msg type to tf2::quaternion type
|
||||
}
|
||||
|
|
@ -83,7 +81,6 @@ void thrust_cb(const mavros_msgs::AttitudeTarget msg){
|
|||
tf2::convert(thrust.orientation,q_Jae); // Convert from msg to quat
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "path_follow");
|
||||
|
|
@ -116,7 +113,7 @@ int main(int argc, char **argv)
|
|||
("mavros/setpoint_raw/attitude",10);
|
||||
|
||||
// Publish attitude cmds in euler angles
|
||||
ros::Publisher euler_pub = nh.advertise<offboard_ex::EulerAngles>
|
||||
ros::Publisher euler_pub = nh.advertise<oscillation_ctrl::EulerAngles>
|
||||
("command/euler_angles",10);
|
||||
|
||||
// Service Clients
|
||||
|
|
@ -126,6 +123,8 @@ int main(int argc, char **argv)
|
|||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("mavros/cmd/takeoff");
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
("status/waypoint_tracker");
|
||||
|
||||
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
|
||||
ros::Rate rate(20.0);
|
||||
|
|
@ -140,18 +139,24 @@ int main(int argc, char **argv)
|
|||
/*********** Initiate variables ************************/
|
||||
// Needed for attitude command
|
||||
mavros_msgs::AttitudeTarget attitude;
|
||||
|
||||
|
||||
attitude.header.frame_id = "map"; // NED Ref Frame(?) #TODO
|
||||
attitude.type_mask = 1|2|4; // Ignore body rates
|
||||
|
||||
// Retrieve parameters from Ctrl_param.yaml file
|
||||
/*std::vector<double> waypoints;
|
||||
nh.getParam("sim/waypoints",waypoints);
|
||||
|
||||
// Need to send pose commands until throttle has been established
|
||||
buff_pose.pose.position.x = waypoints(1);
|
||||
buff_pose.pose.position.y = waypoints(2);
|
||||
buff_pose.pose.position.z = waypoints(3);*/
|
||||
// Retrieve desired waypoints
|
||||
oscillation_ctrl::WaypointTrack wpoint_srv;
|
||||
wpoint_srv.request.query = false;
|
||||
if (waypoint_cl.call(wpoint_srv))
|
||||
{
|
||||
ROS_INFO("Waypoints received");
|
||||
}
|
||||
|
||||
// populate desired waypoints - this is only for original hover as
|
||||
// a change of waypoints is handled by ref_signal.py
|
||||
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
||||
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
||||
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
||||
double alt_des = buff_pose.pose.position.z; // Desired height
|
||||
|
||||
// Desired mode is offboard
|
||||
mavros_msgs::SetMode offb_set_mode;
|
||||
|
|
|
|||
|
|
@ -8,22 +8,17 @@ import numpy as np
|
|||
import time
|
||||
import math
|
||||
|
||||
from pymavlink import mavutil
|
||||
from scipy import signal
|
||||
from scipy.integrate import odeint
|
||||
|
||||
from oscillation_ctrl.msg import tethered_status, RefSignal
|
||||
|
||||
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
|
||||
from controller_msgs.msg import FlatTarget
|
||||
|
||||
from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point
|
||||
|
||||
from geometry_msgs.msg import TwistStamped
|
||||
from geometry_msgs.msg import Pose, Vector3, PoseStamped, Point, TwistStamped
|
||||
from sensor_msgs.msg import Imu
|
||||
|
||||
from mavros_msgs.msg import State
|
||||
|
||||
from pymavlink import mavutil
|
||||
|
||||
class Main:
|
||||
|
||||
def __init__(self):
|
||||
|
|
@ -35,6 +30,7 @@ class Main:
|
|||
self.tstart = rospy.get_time() # Keep track of the start time
|
||||
while self.tstart == 0.0: # Need to make sure get_rostime works
|
||||
self.tstart = rospy.get_time()
|
||||
rospy.loginfo(self.tstart)
|
||||
|
||||
self.dt = 1.0/rate
|
||||
# self.dt = 0.5
|
||||
|
|
@ -71,6 +67,33 @@ class Main:
|
|||
if rospy.has_param('sim/tether_length'):
|
||||
self.tetherL = rospy.get_param('sim/tether_length') # Tether length
|
||||
self.param_exists = True
|
||||
elif rospy.get_time() - self.tstart >= 3.0:
|
||||
self.tetherL = 0.0
|
||||
break
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS #
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Topic, msg type, and class callback method
|
||||
rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
|
||||
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
|
||||
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
|
||||
rospy.Subscriber('/mavros/state', State, self.state_cb)
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# PUBLISHERS
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Publish desired path to compute attitudes
|
||||
self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
|
||||
# Needed for geometric controller to compute thrust
|
||||
self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
|
||||
|
||||
# timer(s), used to control method loop freq(s) as defined by the rate(s)
|
||||
self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# FEEDBACK AND INPUT SHAPING
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
||||
# Smooth path variables
|
||||
self.EPS_F = np.zeros(9) # Final Epsilon/ signal
|
||||
|
|
@ -79,7 +102,12 @@ class Main:
|
|||
# Constants for smooth path generation
|
||||
self.w_tune = 3.13 # 3.13 works well? #########################################################################
|
||||
self.epsilon = 0.7 # Damping ratio
|
||||
self.wn = math.sqrt(9.81/self.tetherL)
|
||||
|
||||
# need exception if we do not have tether:
|
||||
if self.tetherL == 0.0:
|
||||
self.wn = self.w_tune
|
||||
else:
|
||||
self.wn = math.sqrt(9.81/self.tetherL)
|
||||
self.wd = self.wn*math.sqrt(1 - self.epsilon**2)
|
||||
self.k4 = 4*self.epsilon*self.w_tune
|
||||
self.k3 = ((2 + 4*self.epsilon**2)*self.w_tune**2)/self.k4
|
||||
|
|
@ -100,9 +128,9 @@ class Main:
|
|||
#elif rospy.get_time() - self.tstart >= 3.0:
|
||||
# self.xd = np.array([[0],[0],[5.0]]) # make our own if there are no waypoints
|
||||
|
||||
self.xd = Point()
|
||||
self.get_xd = rospy.ServiceProxy('status/waypoint_tracker',WaypointTrack)
|
||||
|
||||
#self.xd = Point()
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
self.empty_point = Point() # Needed to query waypoint_server
|
||||
|
||||
self.des_waypoints = True # True = changing waypoints
|
||||
|
||||
|
|
@ -111,10 +139,6 @@ class Main:
|
|||
self.y0 = [0, 0, 0, 0]
|
||||
self.z0 = [0, 0, 0, 0]
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# FEEDBACK AND INPUT SHAPING
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
||||
# Constants for feedback
|
||||
self.Gd = 0.325
|
||||
self.td = 2*self.Gd*math.pi/self.wd
|
||||
|
|
@ -148,30 +172,6 @@ class Main:
|
|||
self.sk = len(self.SF_delay_x[0]) # from Klausen 2017
|
||||
self.ak = np.zeros(len(self.SF_delay_x[0]))
|
||||
self.s_gain = 0.0 # Gain found from sigmoid
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS #
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Topic, msg type, and class callback method
|
||||
rospy.Subscriber('/status/twoBody_status', tethered_status, self.linkState_cb)
|
||||
rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self.droneVel_cb)
|
||||
rospy.Subscriber('/mavros/imu/data', Imu, self.droneAcc_cb)
|
||||
rospy.Subscriber('/mavros/state', State, self.state_cb)
|
||||
#rospy.Subscriber('/reference/waypoints',Position, self.waypoints_cb) ### Change to position
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# PUBLISHERS
|
||||
# --------------------------------------------------------------------------------#
|
||||
# Publish desired path to compute attitudes
|
||||
self.pub_path = rospy.Publisher('/reference/path',FlatTarget,queue_size = 10)
|
||||
# Needed for geometric controller to compute thrust
|
||||
self.pub_ref = rospy.Publisher('/reference/flatsetpoint',FlatTarget,queue_size = 10)
|
||||
|
||||
# timer(s), used to control method loop freq(s) as defined by the rate(s)
|
||||
self.pub_tim = rospy.Timer(rospy.Duration(1.0/rate), self.publisher)
|
||||
|
||||
# ------------------------------------ _init_ ends ------------------------------ #
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# CALLBACK FUNCTIONS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -230,13 +230,18 @@ class Main:
|
|||
pass
|
||||
|
||||
def waypoints_srv_cb(self):
|
||||
rospy.wait_for_service('status/waypoint_tracker')
|
||||
rospy.wait_for_service('/status/waypoint_tracker')
|
||||
try:
|
||||
self.xd = self.get_xd(True)
|
||||
# if self.des_waypoints == True:
|
||||
|
||||
resp = self.get_xd(False,self.empty_point)
|
||||
self.xd = resp.xd
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
#################################################################
|
||||
#TODO Will need to add a function that gets a message from #
|
||||
# a node which lets refsignal_gen.py know there has been a #
|
||||
# change in xd and therefore runs waypoints_srv_cb again #
|
||||
#################################################################
|
||||
|
||||
# --------------------------------------------------------------------------------#
|
||||
# ALGORITHM
|
||||
|
|
@ -322,9 +327,9 @@ class Main:
|
|||
else:
|
||||
print('No delay')
|
||||
|
||||
# Convolution and input shape filter, and feedback
|
||||
# Convolution of shape filter and trajectory, and feedback
|
||||
def convo(self):
|
||||
|
||||
self.waypoints_srv_cb()
|
||||
# needed for delay function
|
||||
# 1 = determine shapefilter array
|
||||
# 2 = determine theta/phi_fb
|
||||
|
|
@ -332,9 +337,9 @@ class Main:
|
|||
feedBack = 2
|
||||
|
||||
# SOLVE ODE (get ref pos, vel, accel)
|
||||
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd[0],))
|
||||
self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd[1],))
|
||||
self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd[2],))
|
||||
self.x = odeint(self.statespace,self.x0,self.t,args=(self.xd.x,))
|
||||
self.y = odeint(self.statespace,self.y0,self.t,args=(self.xd.y,))
|
||||
self.z = odeint(self.statespace,self.z0,self.t,args=(self.xd.z,))
|
||||
|
||||
for i in range(1,len(self.y0)):
|
||||
self.x[:,i] = np.clip(self.x[:,i], a_min = -self.max[i], a_max = self.max[i])
|
||||
|
|
@ -343,7 +348,7 @@ class Main:
|
|||
|
||||
for j in range(3): # 3 is due to pos, vel, acc. NOT due x, y, z
|
||||
|
||||
self.delay(j,shapeFil) # Determine the delay array
|
||||
self.delay(j,shapeFil) # Determine the delay (shapefilter) array
|
||||
|
||||
if self.SF_idx < len(self.SF_delay_x[0]):
|
||||
self.EPS_I[3*j] = self.x[1,j]
|
||||
|
|
@ -363,9 +368,21 @@ class Main:
|
|||
# Populate EPS_F buffer with desired change based on feedback
|
||||
self.EPS_F[i] = self.EPS_I[i] + EPS_D[i] #+ EPS_D[i]
|
||||
|
||||
# self.x0 = [self.dr_pos.position.x, self.dr_vel.x, self.dr_acc.x, self.x[1,3]]
|
||||
# self.y0 = [self.dr_pos.position.y, self.dr_vel.y, self.dr_acc.y, self.y[1,3]]
|
||||
# self.z0 = [self.dr_pos.position.z, self.dr_vel.z, self.dr_acc.z, self.z[1,3]]
|
||||
# Populate msg with epsilon_final
|
||||
self.ref_sig.header.stamp = rospy.Time.now()
|
||||
self.ref_sig.type_mask = 2 # Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
|
||||
|
||||
self.ref_sig.position.x = self.EPS_F[0]
|
||||
self.ref_sig.position.y = self.EPS_F[1]
|
||||
self.ref_sig.position.z = self.EPS_F[2] + 0.5
|
||||
|
||||
self.ref_sig.velocity.x = self.EPS_F[3]
|
||||
self.ref_sig.velocity.y = self.EPS_F[4]
|
||||
self.ref_sig.velocity.z = self.EPS_F[5]
|
||||
|
||||
self.ref_sig.acceleration.x = self.EPS_F[6]
|
||||
self.ref_sig.acceleration.y = self.EPS_F[7]
|
||||
self.ref_sig.acceleration.z = self.EPS_F[8]
|
||||
|
||||
self.x0 = [self.dr_pos.position.x, self.x[1,1], self.x[1,2], self.x[1,3]]
|
||||
self.y0 = [self.dr_pos.position.y, self.y[1,1], self.y[1,2], self.y[1,3]]
|
||||
|
|
@ -389,37 +406,31 @@ class Main:
|
|||
|
||||
return EPS_D
|
||||
|
||||
def publisher(self,pub_tim):
|
||||
|
||||
# Determine final ref signal
|
||||
self.convo()
|
||||
|
||||
# Populate msg with epsilon_final
|
||||
self.ref_sig.position.x = self.EPS_F[0]
|
||||
self.ref_sig.position.y = self.EPS_F[1]
|
||||
def screen_output(self):
|
||||
|
||||
self.ref_sig.velocity.x = self.EPS_F[3]
|
||||
self.ref_sig.velocity.y = self.EPS_F[4]
|
||||
self.ref_sig.velocity.z = self.EPS_F[5]
|
||||
|
||||
self.ref_sig.acceleration.x = self.EPS_F[6]
|
||||
self.ref_sig.acceleration.y = self.EPS_F[7]
|
||||
self.ref_sig.acceleration.z = self.EPS_F[8]
|
||||
|
||||
# Need typemask = 2 to use correct attitude controller - Jaeyoung Lin
|
||||
self.ref_sig.type_mask = 2
|
||||
|
||||
# Publish command
|
||||
self.ref_sig.header.stamp = rospy.Time.now()
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
self.pub_ref.publish(self.ref_sig)
|
||||
|
||||
# Feedback to user
|
||||
rospy.loginfo(' Var | x | y | z ')
|
||||
rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
|
||||
rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
|
||||
rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
rospy.loginfo('_______________________')
|
||||
|
||||
def publisher(self,pub_tim):
|
||||
|
||||
# Determine final ref signal
|
||||
self.convo()
|
||||
|
||||
# Publish reference signal
|
||||
self.pub_path.publish(self.ref_sig)
|
||||
self.pub_ref.publish(self.ref_sig)
|
||||
|
||||
# Give user feedback on published message:
|
||||
rospy.loginfo(' Var | x | y | z ')
|
||||
rospy.loginfo('Pos: %.2f %.2f %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
|
||||
rospy.loginfo('Vel: %.2f %.2f %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
|
||||
rospy.loginfo('Acc: %.2f %.2f %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
|
||||
rospy.loginfo('_______________________')
|
||||
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
### Script to generate set points which form a square with 2m side lengths
|
||||
import rospy, tf
|
||||
import time
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
|
|
@ -13,11 +13,11 @@ class Main:
|
|||
def __init__(self):
|
||||
|
||||
# variable(s)
|
||||
self.Point = PoseStamped()
|
||||
self.Point = Point()
|
||||
# Init x, y, & z coordinates
|
||||
self.Point.pose.position.x = 0
|
||||
self.Point.pose.position.y = 0
|
||||
self.Point.pose.position.z = 3.5
|
||||
self.Point.x = 0
|
||||
self.Point.y = 0
|
||||
self.Point.z = 3.5
|
||||
|
||||
self.xarray = [1,2,2,2,1,0,0]
|
||||
self.yarray = [0,0,1,2,2,2,1]
|
||||
|
|
@ -30,7 +30,7 @@ class Main:
|
|||
rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)
|
||||
|
||||
# publisher(s), with specified topic, message type, and queue_size
|
||||
self.pub_square = rospy.Publisher('/reference/waypoints', PoseStamped, queue_size = 5)
|
||||
self.pub_square = rospy.Publisher('/reference/waypoints', Point, queue_size = 5)
|
||||
|
||||
# rate(s)
|
||||
pub_rate = 1 # rate for the publisher method, specified in Hz
|
||||
|
|
@ -49,28 +49,28 @@ class Main:
|
|||
else:
|
||||
# Check if i is too large. loop back to first point
|
||||
if self.i >= len(self.xarray):
|
||||
self.Point.pose.position.x = 0
|
||||
self.Point.pose.position.y = 0
|
||||
self.Point.x = 0
|
||||
self.Point.y = 0
|
||||
|
||||
else:
|
||||
self.Point.pose.position.x = self.xarray[self.i]
|
||||
self.Point.pose.position.y = self.yarray[self.i]
|
||||
self.Point.x = self.xarray[self.i]
|
||||
self.Point.y = self.yarray[self.i]
|
||||
|
||||
rospy.loginfo("Sending [Point x] %d [Point y] %d",
|
||||
self.Point.pose.position.x, self.Point.pose.position.y)
|
||||
self.Point.x, self.Point.y)
|
||||
|
||||
# Published desired msgs
|
||||
self.Point.header.stamp = rospy.Time.now()
|
||||
# self.Point.header.stamp = rospy.Time.now()
|
||||
self.pub_square.publish(self.Point)
|
||||
self.j += 1
|
||||
self.i = self.j // self.buffer
|
||||
|
||||
# self.Point.header.stamp = rospy.Time.now()
|
||||
# self.Point.pose.position.x = self.xarray[self.i]
|
||||
# self.Point.pose.position.y = self.yarray[self.i]
|
||||
# self.Point.x = self.xarray[self.i]
|
||||
# self.Point.y = self.yarray[self.i]
|
||||
|
||||
# rospy.loginfo("Sending [Point x] %d [Point y] %d",
|
||||
# self.Point.pose.position.x, self.Point.pose.position.y)
|
||||
# self.Point.x, self.Point.y)
|
||||
|
||||
# Published desired msgs
|
||||
# self.pub_square.publish(self.Point)
|
||||
|
|
|
|||
10
src/step.py
10
src/step.py
|
|
@ -14,9 +14,9 @@ class Main:
|
|||
# variable(s)
|
||||
self.Point = Point()
|
||||
# Init x, y, & z coordinates
|
||||
self.Point.pose.position.x = 1
|
||||
self.Point.pose.position.y = 0
|
||||
self.Point.pose.position.z = 4.0
|
||||
self.Point.x = 1
|
||||
self.Point.y = 0
|
||||
self.Point.z = 4.0
|
||||
|
||||
self.bool = False
|
||||
|
||||
|
|
@ -41,13 +41,13 @@ class Main:
|
|||
if self.bool == False:
|
||||
rospy.loginfo('Waiting...')
|
||||
else:
|
||||
self.Point.header.stamp = rospy.Time.now()
|
||||
#self.Point.header.stamp = rospy.Time.now()
|
||||
|
||||
# Published desired msgs
|
||||
self.pub_step.publish(self.Point)
|
||||
|
||||
rospy.loginfo("Sending [Point x] %d [Point y] %d",
|
||||
self.Point.pose.position.x, self.Point.pose.position.y)
|
||||
self.Point.x, self.Point.y)
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@ class Main:
|
|||
self.xd.x = 0.0
|
||||
self.xd.y = 0.0
|
||||
self.xd.z = 5.0
|
||||
break
|
||||
|
||||
# service(s)
|
||||
|
||||
|
|
@ -47,7 +48,12 @@ class Main:
|
|||
|
||||
|
||||
def waypoint_relay(self,req):
|
||||
self.resp.xd = self.xd
|
||||
#TODO Need to add check to make sure query is boolean
|
||||
if req.query:
|
||||
self.xd = req.xd
|
||||
self.resp.xd = self.xd
|
||||
else:
|
||||
self.resp.xd = self.xd
|
||||
return self.resp
|
||||
|
||||
# Change desired position if there is a change
|
||||
|
|
@ -63,7 +69,9 @@ if __name__=="__main__":
|
|||
rospy.init_node('waypoints_server',anonymous=False)
|
||||
try:
|
||||
obj = Main() # create class object
|
||||
s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)
|
||||
s = rospy.Service('status/waypoint_tracker',WaypointTrack,obj.waypoint_relay)
|
||||
rospy.loginfo_once('waypoints_server has started with:\nxd.x = %.1f\nxd.y = %.1f\nxd.z = %.1f', obj.xd.x, obj.xd.y, obj.xd.z)
|
||||
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
bool query # bool to request waypoints
|
||||
bool query # bool to request or set waypoints
|
||||
geometry_msgs/Point xd # waypoints
|
||||
---
|
||||
geometry_msgs/Point xd # waypoints
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue