forked from cesar.alejandro/oscillation_ctrl
updating to work from home
This commit is contained in:
parent
d9716d0905
commit
53895d5bf3
|
|
@ -3,10 +3,10 @@ launch/mocap_*
|
|||
launch/cortex_bridge.launch
|
||||
src/MoCap_Localization_*.py
|
||||
src/Mocap_*.py
|
||||
src/segmented_tether.py
|
||||
src/killswitch_client.py
|
||||
src/land_client.py
|
||||
msg/Marker.msg
|
||||
msg/Markers.msg
|
||||
launc
|
||||
*.rviz
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
# Ros param when using Klausen Ctrl
|
||||
|
||||
wait: 52
|
||||
wait: 40
|
||||
waypoints: {x: 0.0, y: 0.0, z: 5.0}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
plugin_blacklist:
|
||||
# common
|
||||
#- distance_sensor
|
||||
- safety_area
|
||||
# extras
|
||||
- image_pub
|
||||
|
|
@ -14,3 +15,4 @@ plugin_blacklist:
|
|||
|
||||
plugin_whitelist: []
|
||||
#- 'sys_*'
|
||||
|
||||
|
|
|
|||
|
|
@ -11,6 +11,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<arg name="gazebo_gui" default="false" />
|
||||
<arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
|
||||
<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
|
||||
<arg name="connection_type" default="wifi"/>
|
||||
|
||||
<group if="$(eval connection_type == 'ethernet')">
|
||||
<param name="local_ip" value="192.168.1.175"/>
|
||||
</group>
|
||||
<group if="$(eval connection_type == 'wifi')">
|
||||
<param name="local_ip" value="192.168.1.135"/>
|
||||
</group>
|
||||
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
@ -20,15 +28,15 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
/>
|
||||
|
||||
<!-- Cortex bridge launch -->
|
||||
<include file="$(find cortex_bridge)/launch/cortex_bridge.launch"/>
|
||||
<include file="$(find cortex_bridge)/launch/cortex_bridge.launch">
|
||||
<!--param name="local_ip" value="$(param local_ip)" /-->
|
||||
</include>
|
||||
|
||||
<!-- 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" />
|
||||
<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
|
||||
<!--arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
|
||||
<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" /-->
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
</include>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find oscillation_ctrl)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri_with_tether"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find oscillation_ctrl)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -1,7 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
|
||||
<node name="offb" pkg="offboard_ex" type="offb_node" output="screen"/>
|
||||
|
||||
|
||||
</launch>
|
||||
|
|
@ -4,7 +4,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
/-->
|
||||
<launch>
|
||||
<arg name="model" default="headless_spiri_with_tether"/>
|
||||
<arg name='test' default="none"/>
|
||||
<arg name="test" default="none"/>
|
||||
|
||||
<group ns="sim">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" />
|
||||
|
|
@ -14,6 +14,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
|
||||
|
||||
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
|
|
@ -58,5 +60,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
|
|||
/>
|
||||
</group>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find px4)/launch/$(arg model).launch"/>
|
||||
<include file="$(find oscillation_ctrl)/launch/$(arg model).launch"/>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,26 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="LinkState_phi.py"
|
||||
name="LinkStates"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="ref_signalGen.py"
|
||||
name="RefSignal"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="OscillationDampen"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="pathFollow_node"
|
||||
name="pathFollow_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</launch>
|
||||
|
|
@ -8,10 +8,14 @@
|
|||
<arg name="log_output" default="screen" />
|
||||
<arg name="fcu_protocol" default="v2.0" />
|
||||
<arg name="respawn_mavros" default="false" />
|
||||
<!-- added by Cesar to make changes to our own plugin and config list -->
|
||||
<arg name="pluginlists_yaml" default="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" default="$(find oscillation_ctrl)/config/px4_config.yaml" />
|
||||
|
||||
|
||||
<include file="$(find mavros)/launch/node.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" />
|
||||
<arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
|
||||
<arg name="config_yaml" value="$(arg config_yaml)" />
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
<arg name="tgt_system" value="$(arg tgt_system)" />
|
||||
|
|
|
|||
|
|
@ -0,0 +1,55 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find oscillation_ctrl)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri_with_tether"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find oscillation_ctrl)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -103,13 +103,13 @@ class Main:
|
|||
# gains for thrust PD Controller
|
||||
#self.Kp = 2.7
|
||||
#self.Kd = 3
|
||||
self.Kp = 2.7
|
||||
self.Kp = 3.0
|
||||
self.Kd = 3
|
||||
self.max_a = 14.2
|
||||
self.max_t = self.drone_m*self.max_a
|
||||
self.max_t = self.tot_m*self.max_a
|
||||
self.R = np.empty([3,3]) # rotation matrix
|
||||
self.e3 = np.array([[0],[0],[1]])
|
||||
self.thrust_offset = 0.7 # There was found to be a constant offset
|
||||
self.thrust_offset = 1.0 # There was found to be a constant offset 0.7
|
||||
# --------------------------------------------------------------------------------#
|
||||
# SUBSCRIBERS
|
||||
# --------------------------------------------------------------------------------#
|
||||
|
|
@ -266,7 +266,7 @@ class Main:
|
|||
#thrust = np.dot(np.dot(9.81*self.drone_m,self.e3) - self.Kp*self.error - self.Kd*self.dr_vel,self.R.T[2])/self.max_t
|
||||
#thrust_vector = np.dot(9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel,self.R_e3)/self.max_t
|
||||
#thrust_vector = (9.81*self.drone_m*self.e3 - self.Kp*self.error - self.Kd*self.dr_vel -9.81*self.drone_m*self.path_acc)/self.max_t
|
||||
thrust_vector = (9.81*self.drone_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
|
||||
thrust_vector = (9.81*self.tot_m - self.Kp*self.error[2] - self.Kd*self.dr_vel[2])/self.max_t
|
||||
#thrust = thrust_vector[0]*self.R_e3[0] + thrust_vector[1]*self.R_e3[1] + thrust_vector[2]*self.R_e3[2]
|
||||
thrust = thrust_vector/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
|
||||
#thrust = thrust_vector[2]
|
||||
|
|
@ -370,7 +370,7 @@ class Main:
|
|||
|
||||
# Desired body-oriented forces
|
||||
# shouldnt it be tot_m*path_acc?
|
||||
Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,self.dt*(self.z1 - self.z1_p))
|
||||
Fd = B + G[:3] + self.tot_m*self.dr_acc - np.dot(Kd,z1_dot) - np.dot(Kp,self.z1) - np.dot(Ki,0.5*self.dt*(self.z1 - self.z1_p))
|
||||
|
||||
# Update self.z1_p for "integration"
|
||||
self.z1_p = self.z1
|
||||
|
|
|
|||
|
|
@ -83,7 +83,7 @@ class Main:
|
|||
self.EPS_I = np.zeros(9) # Epsilon shapefilter
|
||||
|
||||
# Constants for smooth path generation
|
||||
self.w_tune = 3.13 # 3.13 works well? #########################################################################
|
||||
self.w_tune = 3 # 3.13 works well? #########################################################################
|
||||
self.epsilon = 0.7 # Damping ratio
|
||||
|
||||
# need exception if we do not have tether:
|
||||
|
|
@ -117,7 +117,7 @@ class Main:
|
|||
self.z0 = [0, 0, 0, 0]
|
||||
|
||||
# Constants for feedback
|
||||
self.Gd = 0.325
|
||||
self.Gd = 0.325 #0.325
|
||||
self.td = 2*self.Gd*math.pi/self.wd
|
||||
|
||||
# Constants for shape filter/ Input shaping
|
||||
|
|
|
|||
|
|
@ -1,88 +0,0 @@
|
|||
#!/usr/bin/env python2.7
|
||||
|
||||
|
||||
### Cesar Rodriguez June 2021
|
||||
### Script to generate set points which form a square with 2m side lengths
|
||||
import rospy, tf
|
||||
import time
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
# class method used to initialize variables once when the program starts
|
||||
def __init__(self):
|
||||
|
||||
# variable(s)
|
||||
self.Point = Point()
|
||||
# Init x, y, & z coordinates
|
||||
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]
|
||||
self.i = 0
|
||||
self.j = 0
|
||||
self.buffer = 10
|
||||
self.bool = False
|
||||
|
||||
self.param_exists = False # check in case param does not exist
|
||||
while self.param_exists == False:
|
||||
if rospy.has_param('status/test_type'):
|
||||
self.test = rospy.get_param('status/test_type')
|
||||
self.param_exists = True
|
||||
|
||||
elif rospy.get_time() - self.tstart >= 3.0 || rospy.get_param('status/test_type') == 'none'
|
||||
self.test = 'none'
|
||||
break
|
||||
|
||||
|
||||
# subscriber(s), with specified topic, message type, and class callback method
|
||||
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', Point, queue_size = 5)
|
||||
|
||||
# rate(s)
|
||||
pub_rate = 1 # rate for the publisher method, specified in Hz
|
||||
|
||||
# timer(s), used to control method loop frequencies as defined by the rate(s)
|
||||
self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)
|
||||
|
||||
def wait_cb(self,data):
|
||||
self.bool = data
|
||||
|
||||
# Publish messages
|
||||
def pub(self,pub_timer):
|
||||
if self.bool == False:
|
||||
rospy.loginfo('Waiting...')
|
||||
else:
|
||||
# Check if i is too large. loop back to first point
|
||||
if self.i >= len(self.xarray):
|
||||
self.Point.x = 0
|
||||
self.Point.y = 0
|
||||
|
||||
else:
|
||||
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.x, self.Point.y)
|
||||
|
||||
# Published desired msgs
|
||||
# self.Point.header.stamp = rospy.Time.now()
|
||||
self.pub_square.publish(self.Point)
|
||||
self.j += 1
|
||||
self.i = self.j // self.buffer
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('square',anonymous=True)
|
||||
try:
|
||||
Main() # create class object
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
62
src/step.py
62
src/step.py
|
|
@ -1,62 +0,0 @@
|
|||
#!/usr/bin/env python2.7
|
||||
|
||||
### Cesar Rodriguez Dec 2021
|
||||
### Generate step input in x or y direction
|
||||
import rospy, tf
|
||||
import time
|
||||
from geometry_msgs.msg import Point
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
# class method used to initialize variables once when the program starts
|
||||
def __init__(self):
|
||||
|
||||
# variable(s)
|
||||
self.Point = Point()
|
||||
# Init x, y, & z coordinates
|
||||
self.Point.x = 5
|
||||
self.Point.y = 0
|
||||
self.Point.z = 3.5 # need to check what the xd was previously and save the height
|
||||
|
||||
self.bool = False
|
||||
|
||||
# subscriber(s)
|
||||
rospy.Subscriber('/status/path_follow',Bool, self.wait_cb)
|
||||
|
||||
# publisher(s), with specified topic, message type, and queue_size
|
||||
self.pub_step = rospy.Publisher('/reference/waypoints', Point, queue_size = 5)
|
||||
|
||||
# rate(s)
|
||||
pub_rate = 1 # rate for the publisher method, specified in Hz
|
||||
|
||||
# timer(s), used to control method loop frequencies as defined by the rate(s)
|
||||
self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.pub)
|
||||
|
||||
# Callbacks
|
||||
def wait_cb(self,data):
|
||||
self.bool = data
|
||||
|
||||
# Publish messages
|
||||
def pub(self,pub_timer):
|
||||
if self.bool == False:
|
||||
rospy.loginfo('Waiting...')
|
||||
else:
|
||||
#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.x, self.Point.y)
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('step',anonymous=True)
|
||||
try:
|
||||
Main() # create class object
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
Loading…
Reference in New Issue