Skip to content

Commit bc498c6

Browse files
jtordejtorde
committed
removed acl-gazebo dependency
1 parent 9acd52e commit bc498c6

File tree

5 files changed

+221
-6
lines changed

5 files changed

+221
-6
lines changed

‎.gitmodules

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,3 @@
1-
[submodule "acl-gazebo"]
2-
path = submodules/acl-gazebo
3-
url = https://gitlab.com/mit-acl/lab/acl-gazebo.git
4-
branch = master
51
[submodule "catkin_simple"]
62
path = submodules/catkin_simple
73
url = https://github.com/catkin/catkin_simple.git

‎mader/launch/mader_specific.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
<node ns="$(arg quad)" name="mader_commands" pkg="mader" type="mader_commands.py" output="screen"/>
1010

1111
<!-- Start Perfect tracker -->
12-
<include file="$(find acl_sim)/launch/perfect_tracker_and_sim.launch">
12+
<include file="$(find mader)/launch/perfect_tracker_and_sim.launch">
1313
<arg name="quad" value="$(arg quad)"/>
1414
<arg name="gazebo" value="false"/>
1515
<arg name="x" value="$(arg x)"/>

‎mader/launch/perfect_tracker_and_sim.launch

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
<launch>
2+
<arg name="quad" default="SQ01s"/>
3+
4+
<!-- Arguments -->
5+
<!--<arg name="joy" default="false"/>-->
6+
<arg name="gazebo" default="true"/>
7+
8+
<arg name="x" default="0" />
9+
<arg name="y" default="0" />
10+
<arg name="z" default="0" />
11+
<arg name="yaw" default="0.0" />
12+
13+
14+
<!-- Spawn vehicle in Gazebo -->
15+
<group if="$(arg gazebo)">
16+
17+
<!-- Spawn vehicle in gazebo -->
18+
<include file="$(find acl_sim)/launch/spawn_quadrotor_with_asus.launch">
19+
<arg name="name" value="$(arg quad)"/>
20+
<arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
21+
<arg name="x" value="$(arg x)"/>
22+
<arg name="y" value="$(arg y)"/>
23+
<arg name="z" value="$(arg z)"/>
24+
<arg name="yaw" default="$(arg yaw)"/>
25+
</include>
26+
27+
<!-- Camera to body tf -->
28+
<node pkg="tf" type="static_transform_publisher" name="camera2body" args="0 0 0 -1.57 0 -1.57 $(arg quad) $(arg quad)/camera 100" />
29+
30+
</group>
31+
32+
33+
<!-- Vicon to world tf -->
34+
<!-- The namespace is needed only for the multi-agent simulations. If not, each agent kills the node of the other one-->
35+
<node ns="$(arg quad)" pkg="tf" type="static_transform_publisher" name="vicon2world" args="0 0 0 0 0 0 /world /vicon 100" />
36+
37+
<!-- Start Perfect Tracker -->
38+
<node ns="$(arg quad)" name="perfect_tracker" pkg="mader" type="perfect_tracker.py" output="screen">
39+
<param name="x" value="$(arg x)"/>
40+
<param name="y" value="$(arg y)"/>
41+
<param name="z" value="$(arg z)"/>
42+
<param name="yaw" value="$(arg yaw)"/>
43+
</node>
44+
45+
</launch>

‎mader/scripts/perfect_tracker.py

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
#!/usr/bin/env python
2+
3+
#Jesus Tordesillas Torres, December 2019
4+
5+
#This files plots in gazebo with the position and orientation of the drone according to the desired position and acceleration specified in the goal topic
6+
7+
import roslib
8+
import rospy
9+
import math
10+
from snapstack_msgs.msg import Goal, State
11+
from geometry_msgs.msg import Pose
12+
from gazebo_msgs.msg import ModelState
13+
import numpy as np
14+
from numpy import linalg as LA
15+
16+
from tf.transformations import quaternion_from_euler, euler_from_quaternion, quaternion_about_axis, quaternion_multiply
17+
18+
from visualization_msgs.msg import Marker
19+
20+
import tf
21+
22+
23+
24+
class FakeSim:
25+
26+
def __init__(self):
27+
self.state=State()
28+
29+
self.state.pos.x = rospy.get_param('~x', 0.0);
30+
self.state.pos.y = rospy.get_param('~y', 0.0);
31+
self.state.pos.z = rospy.get_param('~z', 0.0);
32+
yaw = rospy.get_param('~yaw', 0.0);
33+
34+
pitch=0.0;
35+
roll=0.0;
36+
quat = quaternion_from_euler(yaw, pitch, roll, 'rzyx')
37+
38+
self.state.quat.x = quat[0]
39+
self.state.quat.y = quat[1]
40+
self.state.quat.z = quat[2]
41+
self.state.quat.w = quat[3]
42+
43+
self.pubGazeboState = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
44+
self.pubMarkerDrone = rospy.Publisher('marker', Marker, queue_size=1, latch=True)
45+
self.pubState = rospy.Publisher('state', State, queue_size=1, latch=True)
46+
self.timer = rospy.Timer(rospy.Duration(0.01), self.pubTF)
47+
name = rospy.get_namespace()
48+
self.name = name[1:-1]
49+
50+
rospy.sleep(1.0)
51+
52+
self.state.header.frame_id="world"
53+
self.pubState.publish(self.state)
54+
55+
pose=Pose()
56+
pose.position.x=self.state.pos.x;
57+
pose.position.y=self.state.pos.y;
58+
pose.position.z=self.state.pos.z;
59+
pose.orientation.x = quat[0]
60+
pose.orientation.y = quat[1]
61+
pose.orientation.z = quat[2]
62+
pose.orientation.w = quat[3]
63+
64+
self.pubMarkerDrone.publish(self.getDroneMarker(pose));
65+
66+
67+
68+
69+
def goalCB(self, data):
70+
71+
72+
state = State()
73+
gazebo_state = ModelState()
74+
gazebo_state.model_name = self.name
75+
axis_z=[0,0,1]
76+
77+
accel=[data.a.x, data.a.y, data.a.z + 9.81];
78+
79+
gazebo_state.pose.position.x = data.p.x
80+
gazebo_state.pose.position.y = data.p.y
81+
gazebo_state.pose.position.z = data.p.z
82+
83+
84+
drone_quaternion_with_yaw=[];
85+
86+
if(LA.norm(accel)>0.000001 and LA.norm(np.cross(accel, axis_z))>0.0000001):
87+
norm_accel=LA.norm(accel)
88+
accel=accel/norm_accel
89+
axis=np.cross(accel, axis_z);
90+
91+
dot=np.dot(accel,axis_z)
92+
angle=math.acos(dot)
93+
drone_quaternion = quaternion_about_axis(-angle, axis)# Quaternion(axis=axis, angle=-angle)
94+
95+
#Use the yaw from goal
96+
drone_quaternion_with_yaw=quaternion_multiply(drone_quaternion,quaternion_from_euler(data.yaw, 0.0, 0.0, 'rzyx'))
97+
98+
else: #Take only the yaw angle
99+
drone_quaternion_with_yaw = quaternion_from_euler(data.yaw, 0.0, 0.0, 'rzyx')
100+
101+
gazebo_state.pose.orientation.x = drone_quaternion_with_yaw[0]
102+
gazebo_state.pose.orientation.y = drone_quaternion_with_yaw[1]
103+
gazebo_state.pose.orientation.z = drone_quaternion_with_yaw[2]
104+
gazebo_state.pose.orientation.w = drone_quaternion_with_yaw[3]
105+
106+
107+
108+
#self.gazebo_state.twist = data.twist
109+
110+
## HACK TO NOT USE GAZEBO
111+
gazebo_state.reference_frame = "world"
112+
self.pubGazeboState.publish(gazebo_state)
113+
## END OF HACK TO NOT USE GAZEBO
114+
115+
116+
117+
self.state.header.frame_id="world"
118+
self.state.pos=data.p
119+
self.state.vel=data.v
120+
self.state.quat=gazebo_state.pose.orientation
121+
self.pubState.publish(self.state)
122+
# print("State after:")
123+
# print(self.state.quat)
124+
125+
self.pubMarkerDrone.publish(self.getDroneMarker(gazebo_state.pose));
126+
127+
def pubTF(self, timer):
128+
br = tf.TransformBroadcaster()
129+
br.sendTransform((self.state.pos.x, self.state.pos.y, self.state.pos.z),
130+
(self.state.quat.x, self.state.quat.y, self.state.quat.z, self.state.quat.w),
131+
rospy.Time.now(),
132+
self.name,
133+
"vicon")
134+
135+
def getDroneMarker(self, pose):
136+
marker=Marker();
137+
marker.id=1;
138+
marker.ns="mesh_"+self.name;
139+
marker.header.frame_id="world"
140+
marker.type=marker.MESH_RESOURCE;
141+
marker.action=marker.ADD;
142+
143+
marker.pose=pose
144+
marker.lifetime = rospy.Duration.from_sec(0.0);
145+
marker.mesh_use_embedded_materials=True
146+
marker.mesh_resource="package://acl_sim/meshes/quadrotor/quadrotor.dae"
147+
marker.scale.x=1.0;
148+
marker.scale.y=1.0;
149+
marker.scale.z=1.0;
150+
return marker
151+
152+
153+
154+
155+
156+
def startNode():
157+
c = FakeSim()
158+
rospy.Subscriber("goal", Goal, c.goalCB)
159+
160+
rospy.spin()
161+
162+
if __name__ == '__main__':
163+
164+
ns = rospy.get_namespace()
165+
try:
166+
rospy.init_node('relay')
167+
if str(ns) == '/':
168+
rospy.logfatal("Need to specify namespace as vehicle name.")
169+
rospy.logfatal("This is tyipcally accomplished in a launch file.")
170+
rospy.logfatal("Command line: ROS_NAMESPACE=mQ01 $ rosrun quad_control joy.py")
171+
else:
172+
print "Starting perfect tracker node for: " + ns
173+
startNode()
174+
except rospy.ROSInterruptException:
175+
pass

‎submodules/acl-gazebo

Lines changed: 0 additions & 1 deletion
This file was deleted.

0 commit comments

Comments
 (0)