cesar.alejandro
/
oscillation_ctrl
1
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

updating to work from home

This commit is contained in:
cesar 2022-06-06 17:15:33 -03:00
parent d9716d0905
commit 53895d5bf3
17 changed files with 252 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -3,10 +3,10 @@ launch/mocap_*
launch/cortex_bridge.launch launch/cortex_bridge.launch
src/MoCap_Localization_*.py src/MoCap_Localization_*.py
src/Mocap_*.py src/Mocap_*.py
src/segmented_tether.py
src/killswitch_client.py src/killswitch_client.py
src/land_client.py src/land_client.py
msg/Marker.msg msg/Marker.msg
msg/Markers.msg msg/Markers.msg
launc
*.rviz *.rviz

2
config/Ctrl_param.yaml
View File

@ -1,5 +1,5 @@
# Ros param when using Klausen Ctrl # Ros param when using Klausen Ctrl
wait: 52 wait: 40
waypoints: {x: 0.0, y: 0.0, z: 5.0} waypoints: {x: 0.0, y: 0.0, z: 5.0}

2
config/px4_pluginlists.yaml
View File

@ -1,5 +1,6 @@
plugin_blacklist: plugin_blacklist:
# common # common
#- distance_sensor
- safety_area - safety_area
# extras # extras
- image_pub - image_pub
@ -14,3 +15,4 @@ plugin_blacklist:
plugin_whitelist: [] plugin_whitelist: []
#- 'sys_*' #- 'sys_*'

14
launch/cortex_bridge.launch
View File

@ -11,6 +11,14 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
<arg name="gazebo_gui" default="false" /> <arg name="gazebo_gui" default="false" />
<arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/> <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="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 <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
@ -20,15 +28,15 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
/> />
<!-- Cortex bridge launch --> <!-- 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 --> <!-- MAVROS launch -->
<include file="$(find mavros)/launch/px4.launch"> <include file="$(find mavros)/launch/px4.launch">
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" /> <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="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
<arg name="fcu_protocol" value="$(arg fcu_protocol)" /> <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="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" /> <arg name="gcs_url" value="$(arg gcs_url)" />
</include> </include>

54
launch/headless_spiri.launch Normal file
View File

@ -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>

54
launch/headless_spiri_with_tether.launch Normal file
View File

@ -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>

7
launch/offboard.launch
View File

@ -1,7 +0,0 @@
<?xml version="1.0"?>
<launch>
<node name="offb" pkg="offboard_ex" type="offb_node" output="screen"/>
</launch>

6
launch/oscillation_damp.launch
View File

@ -4,7 +4,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
/--> /-->
<launch> <launch>
<arg name="model" default="headless_spiri_with_tether"/> <arg name="model" default="headless_spiri_with_tether"/>
<arg name='test' default="none"/> <arg name="test" default="none"/>
<group ns="sim"> <group ns="sim">
<rosparam file="$(find oscillation_ctrl)/config/Ctrl_param.yaml" /> <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)"/> <param name="test_type" value="$(arg test)"/>
</group> </group>
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES --> <!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
<node <node
pkg="oscillation_ctrl" pkg="oscillation_ctrl"
@ -58,5 +60,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
/> />
</group> </group>
<!-- PX4 LAUNCH --> <!-- PX4 LAUNCH -->
<include file="$(find px4)/launch/$(arg model).launch"/> <include file="$(find oscillation_ctrl)/launch/$(arg model).launch"/>
</launch> </launch>

26
launch/path_follow.launch
View File

@ -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
launch/px4.launch
View File

@ -8,10 +8,14 @@
<arg name="log_output" default="screen" /> <arg name="log_output" default="screen" />
<arg name="fcu_protocol" default="v2.0" /> <arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" /> <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"> <include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" /> <arg name="pluginlists_yaml" value="$(arg pluginlists_yaml)" />
<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" /> <arg name="config_yaml" value="$(arg config_yaml)" />
<arg name="fcu_url" value="$(arg fcu_url)" /> <arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" /> <arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" /> <arg name="tgt_system" value="$(arg tgt_system)" />

55
launch/spiri.launch Normal file
View File

@ -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>

54
launch/spiri_with_tether.launch Normal file
View File

@ -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>

10
src/klausen_control.py
View File

@ -103,13 +103,13 @@ class Main:
# gains for thrust PD Controller # gains for thrust PD Controller
#self.Kp = 2.7 #self.Kp = 2.7
#self.Kd = 3 #self.Kd = 3
self.Kp = 2.7 self.Kp = 3.0
self.Kd = 3 self.Kd = 3
self.max_a = 14.2 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.R = np.empty([3,3]) # rotation matrix
self.e3 = np.array([[0],[0],[1]]) 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 # 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 = 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 = 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.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[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/(math.cos(self.EulerAng[0]*self.EulerAng[1]))
#thrust = thrust_vector[2] #thrust = thrust_vector[2]
@ -370,7 +370,7 @@ class Main:
# Desired body-oriented forces # Desired body-oriented forces
# shouldnt it be tot_m*path_acc? # 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" # Update self.z1_p for "integration"
self.z1_p = self.z1 self.z1_p = self.z1

4
src/ref_signalGen.py
View File

@ -83,7 +83,7 @@ class Main:
self.EPS_I = np.zeros(9) # Epsilon shapefilter self.EPS_I = np.zeros(9) # Epsilon shapefilter
# Constants for smooth path generation # 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 self.epsilon = 0.7 # Damping ratio
# need exception if we do not have tether: # need exception if we do not have tether:
@ -117,7 +117,7 @@ class Main:
self.z0 = [0, 0, 0, 0] self.z0 = [0, 0, 0, 0]
# Constants for feedback # Constants for feedback
self.Gd = 0.325 self.Gd = 0.325 #0.325
self.td = 2*self.Gd*math.pi/self.wd self.td = 2*self.Gd*math.pi/self.wd
# Constants for shape filter/ Input shaping # Constants for shape filter/ Input shaping

88
src/square.py
View File

@ -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
View File

@ -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