From 798da36dc16c082c1cdaa6719c296c0030eb6e16 Mon Sep 17 00:00:00 2001 From: cesar Date: Fri, 18 Mar 2022 13:13:22 -0300 Subject: [PATCH] small issues fixed --- launch/klausen_dampen.launch | 1 + launch/mocap_sim.launch | 7 ++++--- src/LinkState.py | 38 ++++++++++++++++++++++-------------- src/offb_node.cpp | 2 +- src/ref_signalGen.py | 19 +++++++++--------- src/square.py | 12 ------------ src/step.py | 4 ++-- 7 files changed, 40 insertions(+), 43 deletions(-) diff --git a/launch/klausen_dampen.launch b/launch/klausen_dampen.launch index 24bcb44..8cdbe9c 100644 --- a/launch/klausen_dampen.launch +++ b/launch/klausen_dampen.launch @@ -31,6 +31,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo pkg="oscillation_ctrl" type="ref_signalGen.py" name="refSignal_node" + launch-prefix="xterm -e" /> + - + /> - + diff --git a/src/LinkState.py b/src/LinkState.py index 4a3900d..6adfba3 100755 --- a/src/LinkState.py +++ b/src/LinkState.py @@ -22,6 +22,8 @@ class Main: self.dt = 1.0/rate + rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru + # 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 @@ -64,14 +66,16 @@ class Main: self.service1 = '/gazebo/get_link_state' # need service list to check if models have spawned - self.service_list = rosservice.get_service_list() +# self.service_list = rosservice.get_service_list() # wait for service to exist - while self.service1 not in self.service_list: - print "Waiting for models to spawn..." - self.service_list = rosservice.get_service_list() - if rospy.get_time() - self.tstart >= 10.0: - break + rospy.wait_for_service(self.service1,timeout=10) + +# while self.service1 not in self.service_list: +# print "Waiting for models to spawn..." +# self.service_list = rosservice.get_service_list() +# if rospy.get_time() - self.tstart >= 10.0: +# break # publisher(s) self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1) @@ -141,7 +145,8 @@ class Main: if not self.has_run == 1: if self.pload == True: # Determine yaw offset - self.yaw_offset = drone_Eul[2] + self.yaw_offset = drone_Eul[2] + # Get tether length based off initial displacement self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - @@ -164,17 +169,20 @@ class Main: 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] + #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) + #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] + #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] + + # Get euler angles again for feedback to user + #drone_Eul = self.euler_array(drone_P.link_state.pose.orientation) if self.pload == True: # If there is payload, determine the variables # Pload @@ -213,7 +221,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],4)) + print"Roll: "+str(round(drone_Eul[0]*180/3.14,2)),"\nPitch: "+str(round(drone_Eul[1]*180/3.14,2)),"\nYaw: "+str(round(drone_Eul[2]*180/3.14,2)) 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)) diff --git a/src/offb_node.cpp b/src/offb_node.cpp index 8adf519..2dd0082 100644 --- a/src/offb_node.cpp +++ b/src/offb_node.cpp @@ -114,7 +114,7 @@ int main(int argc, char **argv) // Populate pose msg pose.pose.position.x = 0; pose.pose.position.y = 0; - pose.pose.position.z = 1.0; + pose.pose.position.z = 2.5; pose.pose.orientation.x = q_init.x; pose.pose.orientation.y = q_init.y; pose.pose.orientation.z = q_init.z; diff --git a/src/ref_signalGen.py b/src/ref_signalGen.py index 371a399..e344399 100755 --- a/src/ref_signalGen.py +++ b/src/ref_signalGen.py @@ -409,11 +409,13 @@ class Main: def screen_output(self): # 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('_______________________') + #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('_______________________') + + rospy.loginfo('xd = %.2f',self.xd.x) def publisher(self,pub_tim): @@ -424,12 +426,9 @@ class Main: 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('_______________________') + self.screen_output() if __name__=="__main__": diff --git a/src/square.py b/src/square.py index 73652b0..07e7ed3 100755 --- a/src/square.py +++ b/src/square.py @@ -41,7 +41,6 @@ class Main: def wait_cb(self,data): self.bool = data - # Publish messages def pub(self,pub_timer): if self.bool == False: @@ -65,17 +64,6 @@ class Main: self.j += 1 self.i = self.j // self.buffer -# self.Point.header.stamp = rospy.Time.now() -# 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.pub_square.publish(self.Point) -# self.i += 1 - if __name__=="__main__": # Initiate ROS node diff --git a/src/step.py b/src/step.py index 1301d0c..6a58afc 100755 --- a/src/step.py +++ b/src/step.py @@ -14,9 +14,9 @@ class Main: # variable(s) self.Point = Point() # Init x, y, & z coordinates - self.Point.x = 1 + self.Point.x = 5 self.Point.y = 0 - self.Point.z = 4.0 + self.Point.z = 3.5 # need to check what the xd was previously and save the height self.bool = False