Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
D
Drone ROS Packages
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Board
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Larkin Heintzman
Drone ROS Packages
Commits
c2e326c8
Commit
c2e326c8
authored
Jan 11, 2022
by
Larkin Heintzman
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
plaths last flight push
parent
9325d37e
Changes
13
Hide whitespace changes
Inline
Side-by-side
Showing
13 changed files
with
1193 additions
and
227 deletions
+1193
-227
bus_driver.h
Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h
+12
-1
qualisysPublisher.py
Onboard-SDK-ROS/scripts/qualisysPublisher.py
+2
-2
qualisysSpoofRemote.py
Onboard-SDK-ROS/scripts/qualisysSpoofRemote.py
+82
-0
dji_vehicle_node.cpp
Onboard-SDK-ROS/src/dji_osdk_ros/dji_vehicle_node.cpp
+4
-2
bus_driver.cpp
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
+352
-222
CMakeLists.txt
misson_controller/CMakeLists.txt
+208
-0
package.xml
misson_controller/package.xml
+74
-0
CMakeLists.txt
puredata/CMakeLists.txt
+207
-0
package.xml
puredata/package.xml
+71
-0
crushedPiano.wav
puredata/patches/crushedPiano.wav
+0
-0
drone-osc.pd
puredata/patches/drone-osc.pd
+84
-0
puredataLaunch.bash
puredata/scripts/puredataLaunch.bash
+4
-0
puredataPublisher.py
puredata/scripts/puredataPublisher.py
+93
-0
No files found.
Onboard-SDK-ROS/include/dji_osdk_ros/bus_driver.h
View file @
c2e326c8
...
@@ -111,7 +111,8 @@ ServiceAck obtainCtrlAuthority();
...
@@ -111,7 +111,8 @@ ServiceAck obtainCtrlAuthority();
bool
takeoff
();
bool
takeoff
();
std
::
vector
<
float
>
relativePosition
(
float
x
,
float
y
,
float
z
,
float
yaw
);
void
relativeVelocity
(
float
x
,
float
y
,
float
z
,
float
yaw
,
float
dt
);
void
relativePosition
(
float
x
,
float
y
,
float
z
,
float
yaw
,
float
dt
);
float
applyRotationAngle
(
float
targetYaw
);
float
applyRotationAngle
(
float
targetYaw
);
...
@@ -136,7 +137,17 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
...
@@ -136,7 +137,17 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
void
gpsPosCallback
(
const
sensor_msgs
::
NavSatFix
::
ConstPtr
&
msg
);
void
gpsPosCallback
(
const
sensor_msgs
::
NavSatFix
::
ConstPtr
&
msg
);
void
posFeedbackCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
);
void
posFeedbackCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
);
void
pdPositionCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
);
void
posTrackingCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
);
void
posTrackingCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
);
void
pdMaxSpeedCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdMaxYawSpeedCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdSpeedScaleCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdUpdateRateCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdDampingRateCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdFrequencyRateCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
void
pdAngleOffsetRateCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
);
#endif // MISSION_NODE_H
#endif // MISSION_NODE_H
Onboard-SDK-ROS/scripts/qualisys
Feedback
.py
→
Onboard-SDK-ROS/scripts/qualisys
Publisher
.py
View file @
c2e326c8
...
@@ -14,8 +14,8 @@ def callback(data):
...
@@ -14,8 +14,8 @@ def callback(data):
# we must remapp
# we must remapp
#transform.translation = data.pose.position
#transform.translation = data.pose.position
transform
.
translation
.
x
=
data
.
pose
.
position
.
x
transform
.
translation
.
x
=
data
.
pose
.
position
.
x
transform
.
translation
.
y
=
data
.
pose
.
position
.
z
transform
.
translation
.
y
=
data
.
pose
.
position
.
y
transform
.
translation
.
z
=
data
.
pose
.
position
.
y
transform
.
translation
.
z
=
data
.
pose
.
position
.
z
pub
.
publish
(
transform
)
pub
.
publish
(
transform
)
...
...
Onboard-SDK-ROS/scripts/qualisysSpoofRemote.py
0 → 100755
View file @
c2e326c8
#!/usr/bin/python3
import
rospy
import
numpy
as
np
from
sensor_msgs.msg
import
NavSatFix
from
sensor_msgs.msg
import
Imu
from
std_msgs.msg
import
Float32
from
geometry_msgs.msg
import
PointStamped
from
geometry_msgs.msg
import
QuaternionStamped
from
geometry_msgs.msg
import
Transform
from
geometry_msgs.msg
import
Vector3
from
geometry_msgs.msg
import
Quaternion
import
math
from
pyproj
import
Transformer
class
QualisysSpoof
():
def
__init__
(
self
):
rospy
.
init_node
(
"qualisysSpoof"
)
# zero zero of fake qualisys frame
self
.
anchorPoint
=
rospy
.
get_param
(
"~anchorPoint"
)
rospy
.
Subscriber
(
"dji_osdk_ros/gps_position"
,
NavSatFix
,
self
.
gpsCallback
,
queue_size
=
1
)
rospy
.
Subscriber
(
"dji_osdk_ros/attitude"
,
QuaternionStamped
,
self
.
rotCallback
,
queue_size
=
1
)
# rospy.Subscriber("dji_osdk_ros/height_above_takeoff", Float32, self.heightCallback, queue_size = 1)
# standing in for the feedback qualisys stream
self
.
posePublisher
=
rospy
.
Publisher
(
"feedback"
,
Transform
,
queue_size
=
1
)
self
.
quat
=
Quaternion
()
self
.
transform
=
Transform
()
self
.
startHeightSet
=
False
self
.
startHeight
=
0.0
# self.height = 0.0
def
point_rotation
(
self
,
origin
,
pt
,
ang
):
# returns the pt rotated about the origin by ang (ang in degrees)
c
=
math
.
cos
(
math
.
radians
(
ang
))
s
=
math
.
sin
(
math
.
radians
(
ang
))
# translate to origin
pt_temp
=
[
pt
[
0
]
-
origin
[
0
],
pt
[
1
]
-
origin
[
1
]]
pt_spun
=
[
pt_temp
[
0
]
*
c
-
pt_temp
[
1
]
*
s
,
pt_temp
[
0
]
*
s
+
pt_temp
[
1
]
*
c
]
# translate back to frame
pt_spun
=
[
pt_spun
[
0
]
+
origin
[
0
],
pt_spun
[
1
]
+
origin
[
1
]]
return
pt_spun
def
meters2lat_lon
(
self
,
meters_point
):
# returns point converted to lat lon from meters coordinate system
transformer
=
Transformer
.
from_crs
(
3857
,
4326
)
latlon_point
=
transformer
.
transform
(
meters_point
[
0
],
meters_point
[
1
])
return
latlon_point
def
lat_lon2meters
(
self
,
latlon_point
):
# returns point converted to meters from lat lon coordinate system
transformer
=
Transformer
.
from_crs
(
4326
,
3857
)
meters_point
=
transformer
.
transform
(
latlon_point
[
0
],
latlon_point
[
1
])
return
meters_point
def
rotCallback
(
self
,
data
):
self
.
quat
=
data
.
quaternion
def
gpsCallback
(
self
,
data
):
# convert and compare against home point
apMeters
=
self
.
lat_lon2meters
([
self
.
anchorPoint
[
0
],
self
.
anchorPoint
[
1
]])
cpMeters
=
self
.
lat_lon2meters
([
data
.
latitude
,
data
.
longitude
])
currentOffset
=
[
cpMeters
[
0
]
-
apMeters
[
0
],
cpMeters
[
1
]
-
apMeters
[
1
]]
# rotate current offset by a bit
currentOffset
=
self
.
point_rotation
([
0
,
0
],
currentOffset
,
rospy
.
get_param
(
"~angleOffset"
));
# save init height
if
(
not
self
.
startHeightSet
):
self
.
startHeightSet
=
True
self
.
startHeight
=
data
.
altitude
# performing dark magics
self
.
transform
.
translation
.
x
=
currentOffset
[
1
]
self
.
transform
.
translation
.
y
=
currentOffset
[
0
]
self
.
transform
.
translation
.
z
=
data
.
altitude
-
self
.
startHeight
self
.
transform
.
rotation
=
self
.
quat
self
.
posePublisher
.
publish
(
self
.
transform
)
if
__name__
==
'__main__'
:
qualer
=
QualisysSpoof
()
rospy
.
spin
()
Onboard-SDK-ROS/src/dji_osdk_ros/dji_vehicle_node.cpp
View file @
c2e326c8
...
@@ -516,7 +516,7 @@ bool VehicleNode::initDataSubscribeFromFC()
...
@@ -516,7 +516,7 @@ bool VehicleNode::initDataSubscribeFromFC()
int
nTopic100Hz
=
topicList100Hz
.
size
();
int
nTopic100Hz
=
topicList100Hz
.
size
();
if
(
ptr_wrapper_
->
initPackageFromTopicList
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_100HZ
),
nTopic100Hz
,
if
(
ptr_wrapper_
->
initPackageFromTopicList
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_100HZ
),
nTopic100Hz
,
topicList100Hz
.
data
(),
1
,
10
0
))
topicList100Hz
.
data
(),
1
,
5
0
))
{
{
ack
=
ptr_wrapper_
->
startPackage
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_100HZ
),
WAIT_TIMEOUT
);
ack
=
ptr_wrapper_
->
startPackage
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_100HZ
),
WAIT_TIMEOUT
);
if
(
ACK
::
getError
(
ack
))
if
(
ACK
::
getError
(
ack
))
...
@@ -663,8 +663,10 @@ bool VehicleNode::initDataSubscribeFromFC()
...
@@ -663,8 +663,10 @@ bool VehicleNode::initDataSubscribeFromFC()
topicList400Hz
.
push_back
(
Telemetry
::
TOPIC_HARD_SYNC
);
topicList400Hz
.
push_back
(
Telemetry
::
TOPIC_HARD_SYNC
);
int
nTopic400Hz
=
topicList400Hz
.
size
();
int
nTopic400Hz
=
topicList400Hz
.
size
();
// if (ptr_wrapper_->initPackageFromTopicList(static_cast<int>(SubscribePackgeIndex::PACKAGE_ID_400HZ), nTopic400Hz,
// topicList400Hz.data(), 1, 400))
if
(
ptr_wrapper_
->
initPackageFromTopicList
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_400HZ
),
nTopic400Hz
,
if
(
ptr_wrapper_
->
initPackageFromTopicList
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_400HZ
),
nTopic400Hz
,
topicList400Hz
.
data
(),
1
,
40
0
))
topicList400Hz
.
data
(),
1
,
5
0
))
{
{
ack
=
ptr_wrapper_
->
startPackage
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_400HZ
),
WAIT_TIMEOUT
);
ack
=
ptr_wrapper_
->
startPackage
(
static_cast
<
int
>
(
SubscribePackgeIndex
::
PACKAGE_ID_400HZ
),
WAIT_TIMEOUT
);
if
(
ACK
::
getError
(
ack
))
if
(
ACK
::
getError
(
ack
))
...
...
Onboard-SDK-ROS/src/dji_osdk_ros/samples/bus_driver.cpp
View file @
c2e326c8
...
@@ -51,13 +51,17 @@ ros::ServiceClient waypoint_action_client;
...
@@ -51,13 +51,17 @@ ros::ServiceClient waypoint_action_client;
ros
::
ServiceClient
flight_control_client
;
ros
::
ServiceClient
flight_control_client
;
ros
::
ServiceClient
mission_waypoint_client
;
ros
::
ServiceClient
mission_waypoint_client
;
ros
::
ServiceClient
obtain_ctrl_authority_client
;
ros
::
ServiceClient
obtain_ctrl_authority_client
;
ros
::
Publisher
syncPub
;
// thing to publish to sync topic "positionSync"
// need to take another look at synchronization...
// ros::Publisher syncPub; // thing to publish to sync topic "positionSync"
sensor_msgs
::
NavSatFix
gps_pos
;
// gps position from drone
sensor_msgs
::
NavSatFix
gps_pos
;
// gps position from drone
geometry_msgs
::
Transform
ex_pos
;
// position from external source (qualisys or some such)
geometry_msgs
::
Transform
ex_pos
;
// position from external source (qualisys or some such)
geometry_msgs
::
Transform
track_pos
;
// position from external source (qualisys or some such)
geometry_msgs
::
Transform
track_pos
;
// target position
geometry_msgs
::
Transform
previous_ex_pos
;
// position from external source (qualisys or some such)
geometry_msgs
::
Transform
previous_ex_pos
;
geometry_msgs
::
Transform
previous_track_pos
;
// position from external source (qualisys or some such)
geometry_msgs
::
Transform
previous_track_pos
;
geometry_msgs
::
Transform
pd_pos
;
// desired pose from pd relative to qual center (only x on rotation)
std
::
vector
<
float
>
expectedVel
;
// expected position after applying relative position move
std
::
vector
<
float
>
expectedVel
;
// expected position after applying relative position move
float
feedbackYaw
;
// yaw as measured by pos ref
float
feedbackYaw
;
// yaw as measured by pos ref
float
feedbackPitch
;
// pitch as measured by pos ref
float
feedbackPitch
;
// pitch as measured by pos ref
...
@@ -65,7 +69,7 @@ float feedbackRoll; // roll as measured by pos ref
...
@@ -65,7 +69,7 @@ float feedbackRoll; // roll as measured by pos ref
float
trackingYaw
;
// yaw to track
float
trackingYaw
;
// yaw to track
float
trackingPitch
;
// pitch to track
float
trackingPitch
;
// pitch to track
float
trackingRoll
;
// roll to track
float
trackingRoll
;
// roll to track
int
calibrationWindow
=
25
;
// the last _ position calls that are included in coordinate system calibration
int
calibrationWindow
=
3
;
// the last _ position calls that are included in coordinate system calibration
std
::
vector
<
float
>
calibrationWindowValues
;
std
::
vector
<
float
>
calibrationWindowValues
;
float
angleOffset
;
float
angleOffset
;
float
controlLoopTime
;
float
controlLoopTime
;
...
@@ -78,12 +82,25 @@ bool rotationMatrixCalibratedFlag = false;
...
@@ -78,12 +82,25 @@ bool rotationMatrixCalibratedFlag = false;
bool
gps_home_flag
=
false
;
bool
gps_home_flag
=
false
;
bool
pos_home_flag
=
false
;
bool
pos_home_flag
=
false
;
bool
positionMissionFlag
=
false
;
bool
localMissionFlag
=
false
;
// whether running position mission currently
bool
waypointMissionFlag
=
false
;
bool
gpsMissionFlag
=
false
;
// whether running gps mission currently
bool
positionPlaybackParam
=
false
;
string
missionTypeParam
;
ros
::
Subscriber
gps_pos_subscriber
;
ros
::
Subscriber
gps_pos_subscriber
;
ros
::
Subscriber
pos_feedback_subscriber
;
ros
::
Subscriber
pos_feedback_subscriber
;
ros
::
Subscriber
pos_tracking_subscriber
;
ros
::
Subscriber
pos_tracking_subscriber
;
ros
::
Subscriber
pd_position_subscriber
;
ros
::
Subscriber
pd_heading_subscriber
;
// temp subscribers to pick up puredata parameter information, will be removed once the right parameters are settled on
ros
::
Subscriber
pd_parameter_MinSpeed_subscriber
;
ros
::
Subscriber
pd_parameter_MaxSpeed_subscriber
;
ros
::
Subscriber
pd_parameter_MaxYawSpeed_subscriber
;
ros
::
Subscriber
pd_parameter_SpeedScale_subscriber
;
ros
::
Subscriber
pd_parameter_UpdateRate_subscriber
;
ros
::
Subscriber
pd_parameter_Damping_subscriber
;
ros
::
Subscriber
pd_parameter_Frequency_subscriber
;
ros
::
Subscriber
pd_parameter_AngleOffset_subscriber
;
int
droneId
;
// stupid but will work for now
int
droneId
;
// stupid but will work for now
int
waypointMissionUpdateCounter
=
0
;
// counts iterations before we check waypoints again
int
waypointMissionUpdateCounter
=
0
;
// counts iterations before we check waypoints again
...
@@ -96,15 +113,14 @@ int waypointNumber;
...
@@ -96,15 +113,14 @@ int waypointNumber;
int
currentWaypoint
=
0
;
// how many waypoints have we reached
int
currentWaypoint
=
0
;
// how many waypoints have we reached
long
double
initialWaypointDistance
;
// distance to first waypoint
long
double
initialWaypointDistance
;
// distance to first waypoint
long
double
initialPositionDistance
;
// distance to first position
long
double
initialPositionDistance
;
// distance to first position
double
baseSpeedParam
;
double
waypointSpeedParam
;
double
minSpeedParam
;
double
positionThresholdParam
;
double
angleOffsetParam
;
double
maxSpeedParam
;
double
maxSpeedParam
;
double
speedScalarParam
;
double
speedScalarParam
;
double
minYawSpeedParam
;
double
maxYawSpeedParam
;
double
maxYawSpeedParam
;
// inverse controller variables
// inverse controller variables
ros
::
WallTime
previousWallTime
;
double
dt
;
double
dt
;
double
damping
;
double
damping
;
double
frequency
;
double
frequency
;
...
@@ -119,10 +135,6 @@ double g;
...
@@ -119,10 +135,6 @@ double g;
// converting degrees to radians
// converting degrees to radians
long
double
toRadians
(
const
long
double
degree
)
long
double
toRadians
(
const
long
double
degree
)
{
{
// cmath library in C++
// defines the constant
// M_PI as the value of
// pi accurate to 1e-30
long
double
one_deg
=
(
M_PI
)
/
180
;
long
double
one_deg
=
(
M_PI
)
/
180
;
return
(
one_deg
*
degree
);
return
(
one_deg
*
degree
);
}
}
...
@@ -147,8 +159,6 @@ float eucDistance(std::vector<float> a, std::vector<float> b)
...
@@ -147,8 +159,6 @@ float eucDistance(std::vector<float> a, std::vector<float> b)
long
double
gpsDistance
(
long
double
lat1
,
long
double
long1
,
long
double
lat2
,
long
double
long2
)
long
double
gpsDistance
(
long
double
lat1
,
long
double
long1
,
long
double
lat2
,
long
double
long2
)
{
{
// Convert the latitudes
// Convert the latitudes
// and longitudes
// from degree to radians.
lat1
=
toRadians
(
lat1
);
lat1
=
toRadians
(
lat1
);
long1
=
toRadians
(
long1
);
long1
=
toRadians
(
long1
);
lat2
=
toRadians
(
lat2
);
lat2
=
toRadians
(
lat2
);
...
@@ -164,10 +174,7 @@ long double gpsDistance(long double lat1, long double long1, long double lat2, l
...
@@ -164,10 +174,7 @@ long double gpsDistance(long double lat1, long double long1, long double lat2, l
ans
=
2
*
asin
(
sqrt
(
ans
));
ans
=
2
*
asin
(
sqrt
(
ans
));
// Radius of Earth in
long
double
R
=
6371
;
// radius of earth
// Kilometers, R = 6371
// Use R = 3956 for miles
long
double
R
=
6371
;
// Calculate the result
// Calculate the result
ans
=
ans
*
R
;
ans
=
ans
*
R
;
...
@@ -194,14 +201,12 @@ std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angl
...
@@ -194,14 +201,12 @@ std::vector<std::vector<float>> rotateAxisAngle(std::vector<float> u, float angl
return
rotationMatrix
;
return
rotationMatrix
;
}
}
// Function that return
// dot product of two vector array.
// dot product of two vector array.
float
dotProduct
(
std
::
vector
<
float
>
vect_A
,
std
::
vector
<
float
>
vect_B
)
float
dotProduct
(
std
::
vector
<
float
>
vect_A
,
std
::
vector
<
float
>
vect_B
)
{
{
float
product
=
0
;
float
product
=
0
;
int
vectorSize
=
vect_A
.
size
();
// both vectors must be of same length
int
vectorSize
=
vect_A
.
size
();
// both vectors must be of same length
// Loop for calculate cot product
for
(
int
i
=
0
;
i
<
vectorSize
;
i
++
)
for
(
int
i
=
0
;
i
<
vectorSize
;
i
++
)
product
=
product
+
vect_A
[
i
]
*
vect_B
[
i
];
product
=
product
+
vect_A
[
i
]
*
vect_B
[
i
];
...
@@ -217,7 +222,6 @@ float flatAngleBetween(std::vector<float> a, std::vector<float> b)
...
@@ -217,7 +222,6 @@ float flatAngleBetween(std::vector<float> a, std::vector<float> b)
return
normalizeAngle
(
acosf
(
dotProd
/
(
Magnitude3
(
a
)
*
Magnitude3
(
b
))));
return
normalizeAngle
(
acosf
(
dotProd
/
(
Magnitude3
(
a
)
*
Magnitude3
(
b
))));
}
}
// Function to find
// cross product of two vector array.
// cross product of two vector array.
std
::
vector
<
float
>
crossProduct
(
std
::
vector
<
float
>
vect_A
,
std
::
vector
<
float
>
vect_B
)
std
::
vector
<
float
>
crossProduct
(
std
::
vector
<
float
>
vect_A
,
std
::
vector
<
float
>
vect_B
)
{
{
...
@@ -251,10 +255,7 @@ std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vecto
...
@@ -251,10 +255,7 @@ std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vecto
const
int
n
=
a
.
size
();
// a cols
const
int
n
=
a
.
size
();
// a cols
const
int
m
=
a
[
0
].
size
();
// a rows
const
int
m
=
a
[
0
].
size
();
// a rows
std
::
vector
<
float
>
c
(
n
);
std
::
vector
<
float
>
c
(
n
);
// the PAIN
// the PAIN
// ROS_INFO(" rotation matrix calc:");
for
(
auto
k
=
0
;
k
<
m
;
++
k
)
// row loop
for
(
auto
k
=
0
;
k
<
m
;
++
k
)
// row loop
{
{
for
(
auto
i
=
0
;
i
<
n
;
++
i
)
// col loop
for
(
auto
i
=
0
;
i
<
n
;
++
i
)
// col loop
...
@@ -267,32 +268,38 @@ std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vecto
...
@@ -267,32 +268,38 @@ std::vector<float> MatrixMultiply(std::vector <std::vector<float>> a, std::vecto
return
c
;
return
c
;
}
}
std
::
vector
<
std
::
vector
<
float
>>
rotateAlign
(
std
::
vector
<
float
>
v1
,
std
::
vector
<
float
>
v2
)
// std::vector<std::vector<float>> rotateAlign(std::vector<float> v1, std::vector<float> v2)
{
// {
std
::
vector
<
float
>
axis
=
normalizeVector
(
crossProduct
(
v1
,
v2
));
// std::vector<float> axis = normalizeVector(crossProduct(v1, v2));
float
dotProd
=
dotProduct
(
normalizeVector
(
v1
),
normalizeVector
(
v2
));
// float dotProd = dotProduct(normalizeVector(v1), normalizeVector(v2));
if
(
dotProd
<
-
1.0
)
// if (dotProd < -1.0)
{
// {
dotProd
=
-
1.0
;
// dotProd = -1.0;
}
// }
else
if
(
dotProd
>
1.0
)
// else if (dotProd > 1.0)
{
// {
dotProd
=
1.0
;
// dotProd = 1.0;
}
// }
poseRotationRadians
=
acosf
(
dotProd
);
// poseRotationRadians = acosf(dotProd);
std
::
vector
<
std
::
vector
<
float
>>
result
=
rotateAxisAngle
(
axis
,
poseRotationRadians
);
// returns rotation matrix
// std::vector<std::vector<float>> result = rotateAxisAngle(axis, poseRotationRadians); // returns rotation matrix
//
// ROS_INFO("axis of rotation: (%f, %f, %f)", axis[0], axis[1], axis[2]);
// ROS_INFO("dot product: (%f)", dotProd);
// ROS_INFO("angle rad: (%f)", poseRotationRadians);
// return result;
// }
ROS_INFO
(
"axis of rotation: (%f, %f, %f)"
,
axis
[
0
],
axis
[
1
],
axis
[
2
]);
void
pdPositionCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
)
ROS_INFO
(
"dot product: (%f)"
,
dotProd
);
{
ROS_INFO
(
"angle rad: (%f)"
,
poseRotationRadians
);
pd_pos
=
*
msg
;
// annnnnd thats it folks
return
result
;
}
}
// for those times where we actually fly outside lmao
void
gpsPosCallback
(
const
sensor_msgs
::
NavSatFix
::
ConstPtr
&
msg
)
void
gpsPosCallback
(
const
sensor_msgs
::
NavSatFix
::
ConstPtr
&
msg
)
{
{
gps_pos
=
*
msg
;
gps_pos
=
*
msg
;
if
(
!
waypoint
MissionFlag
)
if
(
!
gps
MissionFlag
)
{
{
return
;
// peace out if not running mission
return
;
// peace out if not running mission
}
}
...
@@ -338,7 +345,7 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
...
@@ -338,7 +345,7 @@ void gpsPosCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
// now update sync topic with progress through waypoints
// now update sync topic with progress through waypoints
std_msgs
::
Float32
syncUpdate
;
std_msgs
::
Float32
syncUpdate
;
syncUpdate
.
data
=
static_cast
<
float32_t
>
(
currentWaypoint
)
+
static_cast
<
float32_t
>
(
static_cast
<
float32_t
>
(
1
)
-
progressDistance
/
nextWaypointDistance
);
syncUpdate
.
data
=
static_cast
<
float32_t
>
(
currentWaypoint
)
+
static_cast
<
float32_t
>
(
static_cast
<
float32_t
>
(
1
)
-
progressDistance
/
nextWaypointDistance
);
syncPub
.
publish
(
syncUpdate
);
//
syncPub.publish(syncUpdate);
if
(
progressDistance
<
tolerance
)
if
(
progressDistance
<
tolerance
)
{
{
...
@@ -359,6 +366,14 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
...
@@ -359,6 +366,14 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
previous_ex_pos
=
ex_pos
;
previous_ex_pos
=
ex_pos
;
ex_pos
=
*
msg
;
// alllllways load the message
ex_pos
=
*
msg
;
// alllllways load the message
// ROS_INFO("Current feedback postion %f, %f, %f", ex_pos.translation.x, ex_pos.translation.y, ex_pos.translation.z);
//
// // for fuckin symetry i guess
// if (!localMissionFlag)
// {
// return; // peace out if not running mission
// }
// FINE ill DO it MYself all ALONE
// FINE ill DO it MYself all ALONE
feedbackYaw
=
atan2
(
2.0
*
(
ex_pos
.
rotation
.
x
*
ex_pos
.
rotation
.
y
+
ex_pos
.
rotation
.
z
*
ex_pos
.
rotation
.
w
),
1
-
2.0
*
(
ex_pos
.
rotation
.
y
*
ex_pos
.
rotation
.
y
+
ex_pos
.
rotation
.
z
*
ex_pos
.
rotation
.
z
));
feedbackYaw
=
atan2
(
2.0
*
(
ex_pos
.
rotation
.
x
*
ex_pos
.
rotation
.
y
+
ex_pos
.
rotation
.
z
*
ex_pos
.
rotation
.
w
),
1
-
2.0
*
(
ex_pos
.
rotation
.
y
*
ex_pos
.
rotation
.
y
+
ex_pos
.
rotation
.
z
*
ex_pos
.
rotation
.
z
));
feedbackPitch
=
asin
(
2.0
*
(
ex_pos
.
rotation
.
x
*
ex_pos
.
rotation
.
z
-
ex_pos
.
rotation
.
w
*
ex_pos
.
rotation
.
y
));
feedbackPitch
=
asin
(
2.0
*
(
ex_pos
.
rotation
.
x
*
ex_pos
.
rotation
.
z
-
ex_pos
.
rotation
.
w
*
ex_pos
.
rotation
.
y
));
...
@@ -367,22 +382,24 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
...
@@ -367,22 +382,24 @@ void posFeedbackCallback(const geometry_msgs::Transform::ConstPtr& msg)
void
posTrackingCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
)
void
posTrackingCallback
(
const
geometry_msgs
::
Transform
::
ConstPtr
&
msg
)
{
{
previous_track_pos
=
track_pos
;
previous_track_pos
=
track_pos
;
track_pos
=
*
msg
;
track_pos
=
*
msg
;
// FINE ill DO it MYself all ALONE
// // for fuckin symetry i guess
// if (!localMissionFlag)
// {
// return; // peace out if not running mission
// }
trackingYaw
=
atan2
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
y
+
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
w
),
1
-
2.0
*
(
track_pos
.
rotation
.
y
*
track_pos
.
rotation
.
y
+
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
z
));
trackingYaw
=
atan2
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
y
+
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
w
),
1
-
2.0
*
(
track_pos
.
rotation
.
y
*
track_pos
.
rotation
.
y
+
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
z
));
trackingPitch
=
asin
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
z
-
track_pos
.
rotation
.
w
*
track_pos
.
rotation
.
y
));
trackingPitch
=
asin
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
z
-
track_pos
.
rotation
.
w
*
track_pos
.
rotation
.
y
));
trackingRoll
=
atan2
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
w
+
track_pos
.
rotation
.
y
*
track_pos
.
rotation
.
z
),
1
-
2.0
*
(
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
z
+
track_pos
.
rotation
.
w
*
track_pos
.
rotation
.
w
));
trackingRoll
=
atan2
(
2.0
*
(
track_pos
.
rotation
.
x
*
track_pos
.
rotation
.
w
+
track_pos
.
rotation
.
y
*
track_pos
.
rotation
.
z
),
1
-
2.0
*
(
track_pos
.
rotation
.
z
*
track_pos
.
rotation
.
z
+
track_pos
.
rotation
.
w
*
track_pos
.
rotation
.
w
));
}
}
bool
setupWaypointMission
(
int
responseTimeout
)
bool
setupWaypointMission
(
int
responseTimeout
)
{
{
ros
::
spinOnce
();
ros
::
spinOnce
();
// ROS_INFO();
while
(
!
ros
::
param
::
has
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/numWaypoints"
))
while
(
!
ros
::
param
::
has
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/numWaypoints"
))
{
{
ROS_INFO
(
"waiting for waypoints to load..."
);
ROS_INFO
(
"waiting for waypoints to load..."
);
...
@@ -432,8 +449,8 @@ bool runWaypointMission()
...
@@ -432,8 +449,8 @@ bool runWaypointMission()
return
false
;
return
false
;
}
}
waypoint
MissionFlag
=
true
;
gps
MissionFlag
=
true
;
position
MissionFlag
=
false
;
local
MissionFlag
=
false
;
return
true
;
return
true
;
}
}
...
@@ -441,26 +458,24 @@ bool runWaypointMission()
...
@@ -441,26 +458,24 @@ bool runWaypointMission()
bool
runPositionMission
()
bool
runPositionMission
()
{
{
std
::
vector
<
float
>
velocityCommand
{
0.0
,
0.0
,
0.0
};
std
::
vector
<
float
>
velocityCommand
{
0.0
,
0.0
,
0.0
};
ros
::
Rate
updateRate
(
updateRateParam
);
// update period for topics/commands
localMissionFlag
=
true
;
positionMissionFlag
=
tru
e
;
gpsMissionFlag
=
fals
e
;
int
currentPositionIndex
=
0
;
int
currentPositionIndex
=
0
;
int
numPositions
;
int
numPositions
;
ros
::
param
::
get
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/numPositions"
,
numPositions
);
ros
::
param
::
get
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/numPositions"
,
numPositions
);
while
(
position
MissionFlag
&&
ros
::
ok
())
while
(
local
MissionFlag
&&
ros
::
ok
())
{
{
ros
::
Rate
updateRate
(
updateRateParam
);
// let topics update
// let topics update
ros
::
spinOnce
();
ros
::
spinOnce
();
dt
=
controlLoopTime
;
dt
=
(
ros
::
WallTime
::
now
()
-
previousWallTime
).
toNSec
()
*
1e-9
;
g
=
1.0
/
(
1.0
+
kd
*
dt
+
kp
*
dt
*
dt
);
g
=
1
/
(
1
+
kd
*
dt
+
kp
*
dt
*
dt
);
ksg
=
kp
*
g
;
ksg
=
kp
*
g
;
kdg
=
(
kd
+
kp
*
dt
)
*
g
;
kdg
=
(
kd
+
kp
*
dt
)
*
g
;
previousWallTime
=
ros
::
WallTime
::
now
();
std
::
vector
<
float
>
currentPos
std
::
vector
<
float
>
currentPos
{
{
...
@@ -468,6 +483,7 @@ bool runPositionMission()
...
@@ -468,6 +483,7 @@ bool runPositionMission()
static_cast
<
float
>
(
ex_pos
.
translation
.
y
),
static_cast
<
float
>
(
ex_pos
.
translation
.
y
),
static_cast
<
float
>
(
ex_pos
.
translation
.
z
)
static_cast
<
float
>
(
ex_pos
.
translation
.
z
)
};
};
ROS_INFO
(
"Current postion %f, %f, %f"
,
currentPos
[
0
],
currentPos
[
1
],
currentPos
[
2
]);
std
::
vector
<
float
>
currentVel
std
::
vector
<
float
>
currentVel
{
{
...
@@ -480,16 +496,17 @@ bool runPositionMission()
...
@@ -480,16 +496,17 @@ bool runPositionMission()
std
::
vector
<
float
>
targetVel
;
std
::
vector
<
float
>
targetVel
;
float
targetYaw
;
float
targetYaw
;
// check expected velocity verses actual velocity
// wip, just align w external reference
if
(
Magnitude3
(
velocityCommand
)
!=
0.0
&&
Magnitude3
(
currentVel
)
!=
0.0
)
// check expected velocity verses actual velocity
{
// if (Magnitude3(velocityCommand) != 0.0 && Magnitude3(currentVel) != 0.0)
float
ang
=
flatAngleBetween
(
velocityCommand
,
currentVel
);
// {
updateAngleScaler
(
ang
);
// float ang = flatAngleBetween(velocityCommand, currentVel);
ROS_INFO
(
"new angle discrepancy!: %f"
,
angleOffset
);
// updateAngleScaler(ang);
}
// ROS_INFO("new angle discrepancy: %f", angleOffset);
// }
// next depends on what mode we're in ------------------------------------
// next depends on what mode we're in ------------------------------------
if
(
positionPlaybackParam
)
if
(
missionTypeParam
.
compare
(
"tracking"
)
==
0
)
{
{
targetPos
=
std
::
vector
<
float
>
targetPos
=
std
::
vector
<
float
>
{
{
...
@@ -507,7 +524,19 @@ bool runPositionMission()
...
@@ -507,7 +524,19 @@ bool runPositionMission()
targetYaw
=
trackingYaw
;
targetYaw
=
trackingYaw
;
}
}
else
else
if
(
missionTypeParam
.
compare
(
"puredata"
)
==
0
)
{
// let pure data handle positioning, we just load from topic
targetPos
=
std
::
vector
<
float
>
{
static_cast
<
float
>
(
pd_pos
.
translation
.
x
),
static_cast
<
float
>
(
pd_pos
.
translation
.
y
),
static_cast
<
float
>
(
pd_pos
.
translation
.
z
)
};
targetYaw
=
pd_pos
.
rotation
.
x
;
targetVel
=
std
::
vector
<
float
>
{
0.0
,
0.0
,
0.0
};
}
else
if
(
missionTypeParam
.
compare
(
"points"
)
==
0
)
{
{
std
::
vector
<
float
>
tempPos
;
std
::
vector
<
float
>
tempPos
;
ros
::
param
::
get
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/positions/position_"
+
to_string
(
currentPositionIndex
),
tempPos
);
ros
::
param
::
get
(
"/robot_list/robot_"
+
to_string
(
droneId
)
+
"/positions/position_"
+
to_string
(
currentPositionIndex
),
tempPos
);
...
@@ -517,27 +546,26 @@ bool runPositionMission()
...
@@ -517,27 +546,26 @@ bool runPositionMission()
tempPos
[
2
],
tempPos
[
2
],
tempPos
[
3
]
tempPos
[
3
]
};
};
//----------------------------------
targetVel
=
std
::
vector
<
float
>
{
0.0
,
0.0
,
0.0
};
targetVel
=
std
::
vector
<
float
>
{
0.0
,
0.0
,
0.0
};
targetYaw
=
normalizeAngle
((
M_PI
/
180.0
)
*
tempPos
[
4
]);
//----------------------------------
targetYaw
=
normalizeAngle
((
M_PI
/
180.0
)
*
tempPos
[
4
])
+
angleOffsetParam
;
// check if we're close to position
// check if we're close to position
float
checkDist
=
eucDistance
(
currentPos
,
targetPos
);
float
checkDist
=
eucDistance
(
currentPos
,
targetPos
);
if
(
checkDist
<=
0.2
)
//&& abs(currentYaw - targetYaw) <= 0.1) // close ish
if
(
checkDist
<=
positionThresholdParam
)
//&& abs(currentYaw - targetYaw) <= 0.1) // close ish
{
{
currentPositionIndex
+=
1
;
// go to next position
currentPositionIndex
+=
1
;
// go to next position
}
}
if
(
currentPositionIndex
>=
numPositions
)
if
(
currentPositionIndex
>=
numPositions
)
{
{
position
MissionFlag
=
false
;
local
MissionFlag
=
false
;
}
}
}
}
float
currentYaw
=
feedbackYaw
;
float
currentYaw
=
feedbackYaw
;
ROS_INFO
(
"Current postion %f, %f, %f"
,
currentPos
[
0
],
currentPos
[
1
],
currentPos
[
2
]);
ROS_INFO
(
"Target position %f, %f, %f"
,
targetPos
[
0
],
targetPos
[
1
],
targetPos
[
2
]);
ROS_INFO
(
"Target position %f, %f, %f"
,
targetPos
[
0
],
targetPos
[
1
],
targetPos
[
2
]);
ROS_INFO
(
"Target velocity %f, %f, %f"
,
targetVel
[
0
],
targetVel
[
1
],
targetVel
[
2
]);
ROS_INFO
(
"Target yaw: %f"
,
targetYaw
);
ROS_INFO
(
"Target yaw: %f"
,
targetYaw
);
ROS_INFO
(
"Current yaw: %f"
,
currentYaw
);
ROS_INFO
(
"Current yaw: %f"
,
currentYaw
);
ROS_INFO
(
"Elapsed frame time: %f"
,
static_cast
<
float
>
(
dt
));
ROS_INFO
(
"Elapsed frame time: %f"
,
static_cast
<
float
>
(
dt
));
...
@@ -548,29 +576,136 @@ bool runPositionMission()
...
@@ -548,29 +576,136 @@ bool runPositionMission()
std
::
vector
<
float
>
force
std
::
vector
<
float
>
force
{
{
// using commanded velocity instead of actual
// using commanded velocity instead of actual
static_cast
<
float
>
((
(
targetPos
[
0
]
-
currentPos
[
0
])
*
ksg
+
(
targetVel
[
0
]
-
velocityCommand
[
0
])
*
kdg
)
*
speedScalarParam
),
static_cast
<
float
>
((
targetPos
[
0
]
-
currentPos
[
0
])
*
ksg
+
(
targetVel
[
0
]
-
velocityCommand
[
0
])
*
kdg
),
static_cast
<
float
>
((
(
targetPos
[
1
]
-
currentPos
[
1
])
*
ksg
+
(
targetVel
[
1
]
-
velocityCommand
[
1
])
*
kdg
)
*
speedScalarParam
),
static_cast
<
float
>
((
targetPos
[
1
]
-
currentPos
[
1
])
*
ksg
+
(
targetVel
[
1
]
-
velocityCommand
[
1
])
*
kdg
),
static_cast
<
float
>
((
(
targetPos
[
2
]
-
currentPos
[
2
])
*
ksg
+
(
targetVel
[
2
]
-
velocityCommand
[
2
])
*
kdg
)
*
speedScalarParam
)
static_cast
<
float
>
((
targetPos
[
2
]
-
currentPos
[
2
])
*
ksg
+
(
targetVel
[
2
]
-
velocityCommand
[
2
])
*
kdg
)
};
};
// velocityCommand = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, angleOffset), velocityCommand);
ROS_INFO
(
"target velocity: %f, %f, %f"
,
targetVel
[
0
],
targetVel
[
1
],
targetVel
[
2
]);
force
=
MatrixMultiply
(
rotateAxisAngle
(
std
::
vector
<
float
>
{
0.0
,
0.0
,
1.0
},
angleOffsetParam
),
force
);
ROS_INFO
(
"current velocity: %f, %f, %f"
,
currentVel
[
0
],
currentVel
[
1
],
currentVel
[
2
]);
velocityCommand
=
std
::
vector
<
float
>
{
velocityCommand
[
0
]
*
speedScalarParam
+
dt
*
force
[
0
],
velocityCommand
[
1
]
*
speedScalarParam
+
dt
*
force
[
1
],
velocityCommand
[
2
]
*
speedScalarParam
+
dt
*
force
[
2
]};
velocityCommand
=
std
::
vector
<
float
>
{
velocityCommand
[
0
]
+
dt
*
force
[
0
],
velocityCommand
[
1
]
+
dt
*
force
[
1
],
velocityCommand
[
2
]
+
dt
*
force
[
2
]};
if
(
Magnitude3
(
velocityCommand
)
>
maxSpeedParam
)
if
(
Magnitude3
(
velocityCommand
)
>
maxSpeedParam
)
{
{
velocityCommand
=
normalizeVector
(
velocityCommand
);
velocityCommand
=
normalizeVector
(
velocityCommand
);
velocityCommand
=
std
::
vector
<
float
>
{
maxSpeedParam
*
velocityCommand
[
0
],
maxSpeedParam
*
velocityCommand
[
1
],
maxSpeedParam
*
velocityCommand
[
2
]};
velocityCommand
=
std
::
vector
<
float
>
{
maxSpeedParam
*
velocityCommand
[
0
],
maxSpeedParam
*
velocityCommand
[
1
],
maxSpeedParam
*
velocityCommand
[
2
]};
ROS_INFO
(
"normalizing velocity command..."
);
}
}
velocityCommand
=
MatrixMultiply
(
rotateAxisAngle
(
std
::
vector
<
float
>
{
0.0
,
0.0
,
1.0
},
angleOffset
),
velocityCommand
);
ROS_INFO
(
"calling vel command %f, %f, %f"
,
velocityCommand
[
0
],
velocityCommand
[
1
],
velocityCommand
[
2
]);
ROS_INFO
(
"calling vel command %f, %f, %f"
,
velocityCommand
[
0
],
velocityCommand
[
1
],
velocityCommand
[
2
]);
expectedVel
=
relativePosition
(
velocityCommand
[
0
],
velocityCommand
[
1
],
velocityCommand
[
2
],
yawCommand
);
relativeVelocity
(
velocityCommand
[
0
],
velocityCommand
[
1
],
velocityCommand
[
2
],
yawCommand
,
dt
);
updateRate
.
sleep
();
}
}
return
true
;
return
true
;
}
}
void
relativeVelocity
(
float
x
,
float
y
,
float
z
,
float
yaw
,
float
dt
)
{
// convert relative position to velocity command
std
::
vector
<
float
>
posVec
{
x
,
y
,
z
};
float
totalSpeed
=
Magnitude3
(
posVec
);
float
speed
;
// float dt = controlLoopTime*1000.0;
if
(
totalSpeed
>
maxSpeedParam
)
{
speed
=
maxSpeedParam
;
}
else
{
speed
=
totalSpeed
;
}
std
::
vector
<
float
>
velDir
=
normalizeVector
(
posVec
);
std
::
vector
<
float
>
velVec
=
{
velDir
[
0
]
*
speed
,
velDir
[
1
]
*
speed
,
velDir
[
2
]
*
speed
};
dji_osdk_ros
::
FlightTaskControl
flightTaskControl
;
dji_osdk_ros
::
JoystickParams
joystickParams
;
joystickParams
.
x
=
velVec
[
0
];
joystickParams
.
y
=
velVec
[
1
];
joystickParams
.
z
=
velVec
[
2
];
if
(
abs
(
yaw
)
>
maxYawSpeedParam
)
{
if
(
yaw
<
0
)
{
yaw
=
-
maxYawSpeedParam
;
}
else
{
yaw
=
maxYawSpeedParam
;
}
}
joystickParams
.
yaw
=
(
180.0
/
M_PI
)
*
yaw
;
ROS_INFO
(
"rel vel: (yaw rate: %fdeg/s, speed: %fm/s, over %fsec, going %fm)"
,
joystickParams
.
yaw
,
speed
,
dt
,
totalSpeed
);
ROS_INFO
(
"---------------
\n
"
);
flightTaskControl
.
request
.
task
=
dji_osdk_ros
::
FlightTaskControl
::
Request
::
TASK_VELOCITY_AND_YAWRATE_CONTROL
;
flightTaskControl
.
request
.
joystickCommand
=
joystickParams
;
flightTaskControl
.
request
.
velocityControlTimeMs
=
dt
;
flightTaskControl
.
request
.
posThresholdInM
=
0.1
;
flightTaskControl
.
request
.
yawThresholdInDeg
=
1
;
flight_control_client
.
call
(
flightTaskControl
);
// actually waits for drone to "reach" position
// return flightTaskControl.response.result;
// return std::vector<float> {(dt)*velVec[0], (dt)*velVec[1], (dt)*velVec[2]};
return
;
}
void
relativePosition
(
float
x
,
float
y
,
float
z
,
float
yaw
,
float
dt
)
{
// // convert relative position to velocity command
std
::
vector
<
float
>
posVec
{
x
,
y
,
z
};
float
totalDist
=
Magnitude3
(
posVec
);
// float speed;
// // float dt = controlLoopTime*1000.0;
// if ((totalDist/dt) > maxSpeedParam)
// {
// speed = maxSpeedParam;
// }
// else
// {
// speed = totalDist/dt;
// }
//
// std::vector<float> velDir = normalizeVector(posVec);
// std::vector<float> velVec = {velDir[0]*speed, velDir[1]*speed, velDir[2]*speed};
//
dji_osdk_ros
::
FlightTaskControl
flightTaskControl
;
dji_osdk_ros
::
JoystickParams
joystickParams
;
joystickParams
.
x
=
x
;
joystickParams
.
y
=
y
;
joystickParams
.
z
=
z
;
//
// if (abs(yaw) > maxYawSpeedParam)
// {
// if (yaw < 0)
// {
// yaw = -maxYawSpeedParam;
// }
// else
// {
// yaw = maxYawSpeedParam;
// }
// }
joystickParams
.
yaw
=
(
180.0
/
M_PI
)
*
yaw
;
ROS_INFO
(
"rel pos: (yaw rate: %fdeg/s, speed: %fm/s, over %fsec, going %fm)"
,
joystickParams
.
yaw
,
totalDist
/
dt
,
dt
,
totalDist
);
ROS_INFO
(
"---------------
\n
"
);
flightTaskControl
.
request
.
task
=
dji_osdk_ros
::
FlightTaskControl
::
Request
::
TASK_POSITION_AND_YAW_CONTROL
;
flightTaskControl
.
request
.
joystickCommand
=
joystickParams
;
flightTaskControl
.
request
.
velocityControlTimeMs
=
dt
;
flightTaskControl
.
request
.
posThresholdInM
=
0.1
;
flightTaskControl
.
request
.
yawThresholdInDeg
=
1
;
flight_control_client
.
call
(
flightTaskControl
);
// actually waits for drone to "reach" position
// return flightTaskControl.response.result;
// return std::vector<float> {(dt)*velVec[0], (dt)*velVec[1], (dt)*velVec[2]};
return
;
}
void
setWaypointDefaults
(
WayPointSettings
*
wp
)
void
setWaypointDefaults
(
WayPointSettings
*
wp
)
{
{
wp
->
damping
=
0.25
;
wp
->
damping
=
0.25
;
...
@@ -590,8 +725,8 @@ void setWaypointDefaults(WayPointSettings* wp)
...
@@ -590,8 +725,8 @@ void setWaypointDefaults(WayPointSettings* wp)
void
setWaypointInitDefaults
(
dji_osdk_ros
::
MissionWaypointTask
&
waypointTask
)
void
setWaypointInitDefaults
(
dji_osdk_ros
::
MissionWaypointTask
&
waypointTask
)
{
{
waypointTask
.
velocity_range
=
base
SpeedParam
;
waypointTask
.
velocity_range
=
waypoint
SpeedParam
;
waypointTask
.
idle_velocity
=
base
SpeedParam
;
waypointTask
.
idle_velocity
=
waypoint
SpeedParam
;
waypointTask
.
action_on_finish
=
dji_osdk_ros
::
MissionWaypointTask
::
FINISH_NO_ACTION
;
waypointTask
.
action_on_finish
=
dji_osdk_ros
::
MissionWaypointTask
::
FINISH_NO_ACTION
;
waypointTask
.
mission_exec_times
=
1
;
waypointTask
.
mission_exec_times
=
1
;
waypointTask
.
yaw_mode
=
dji_osdk_ros
::
MissionWaypointTask
::
YAW_MODE_RC
;
waypointTask
.
yaw_mode
=
dji_osdk_ros
::
MissionWaypointTask
::
YAW_MODE_RC
;
...
@@ -684,10 +819,6 @@ ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
...
@@ -684,10 +819,6 @@ ServiceAck missionAction(DJI::OSDK::DJI_MISSION_TYPE type,
missionWpAction
.
response
.
cmd_set
,
missionWpAction
.
response
.
cmd_set
,
missionWpAction
.
response
.
cmd_id
,
missionWpAction
.
response
.
cmd_id
,
missionWpAction
.
response
.
ack_data
};
missionWpAction
.
response
.
ack_data
};
// switch (type)
// {
// case DJI::OSDK::WAYPOINT:
// }
}
}
bool
takeoff
()
bool
takeoff
()
...
@@ -708,85 +839,19 @@ bool land()
...
@@ -708,85 +839,19 @@ bool land()
return
flightTaskControl
.
response
.
result
;
return
flightTaskControl
.
response
.
result
;
}
}
std
::
vector
<
float
>
relativePosition
(
float
x
,
float
y
,
float
z
,
float
yaw
)
{
// convert relative position to velocity command
std
::
vector
<
float
>
posVec
{
x
,
y
,
z
};
// TESTING SMALL ANGLE ERROR IN PLACEMENT OF DRONE
// ------------------------------
// posVec = MatrixMultiply(rotateAxisAngle(std::vector<float> {0.0, 0.0, 1.0}, (M_PI/180.0)*180.0), posVec);
float
totalDist
=
Magnitude3
(
posVec
);
float
speed
;
float
totalTimeMs
;
// first check if we can make it in the allotted time
if
((
totalDist
/
controlLoopTime
)
>
maxSpeedParam
)
{
speed
=
maxSpeedParam
;
totalTimeMs
=
controlLoopTime
*
1000.0
;
}
else
{
speed
=
max
(
minSpeedParam
,
static_cast
<
double
>
(
totalDist
/
controlLoopTime
));
totalTimeMs
=
(
totalDist
/
speed
)
*
1000.0
;
// convert from s to ms, aim just short of position
}
std
::
vector
<
float
>
velDir
=
normalizeVector
(
posVec
);
std
::
vector
<
float
>
velVec
=
{
velDir
[
0
]
*
speed
,
velDir
[
1
]
*
speed
,
velDir
[
2
]
*
speed
};
dji_osdk_ros
::
FlightTaskControl
flightTaskControl
;
dji_osdk_ros
::
JoystickParams
joystickParams
;
joystickParams
.
x
=
velVec
[
0
];
joystickParams
.
y
=
velVec
[
1
];
joystickParams
.
z
=
velVec
[
2
];
if
(
abs
(
yaw
)
>=
0.05
)
{
float
tempYawRate
=
(
180.0
/
M_PI
)
*
yaw
/
(
totalTimeMs
/
1000.0
);
if
(
abs
(
tempYawRate
)
>
maxYawSpeedParam
)
{
tempYawRate
=
(
tempYawRate
/
abs
(
tempYawRate
))
*
maxYawSpeedParam
;
// if distance is small, let yaw dictate duration
// if (totalDist <= 0.1)
// {
// totalTimeMs = (abs(yaw)/tempYawRate)*1000.0;
// }
}
joystickParams
.
yaw
=
tempYawRate
;
}
else
{
joystickParams
.
yaw
=
0.0
;
}
ROS_INFO
(
"yaw rate: %f, speed: %f, over %f sec, going %fm"
,
joystickParams
.
yaw
,
speed
,
(
totalTimeMs
/
1000.0
),
totalDist
);
ROS_INFO
(
"---------------
\n
"
);
flightTaskControl
.
request
.
task
=
dji_osdk_ros
::
FlightTaskControl
::
Request
::
TASK_VELOCITY_AND_YAWRATE_CONTROL
;
flightTaskControl
.
request
.
joystickCommand
=
joystickParams
;
flightTaskControl
.
request
.
velocityControlTimeMs
=
totalTimeMs
;
flightTaskControl
.
request
.
posThresholdInM
=
0.1
;
flightTaskControl
.
request
.
yawThresholdInDeg
=
1
;
flight_control_client
.
call
(
flightTaskControl
);
// actually waits for drone to "reach" position
// return flightTaskControl.response.result;
return
std
::
vector
<
float
>
{(
totalTimeMs
/
1000.0
)
*
velVec
[
0
],
(
totalTimeMs
/
1000.0
)
*
velVec
[
1
],
(
totalTimeMs
/
1000.0
)
*
velVec
[
2
]};
}
bool
stopMotion
()
bool
stopMotion
()
{
{
if
(
waypoint
MissionFlag
)
if
(
gps
MissionFlag
)
{
{
dji_osdk_ros
::
MissionWpAction
waypointAction
;
dji_osdk_ros
::
MissionWpAction
waypointAction
;
waypointAction
.
request
.
action
=
dji_osdk_ros
::
MissionWpAction
::
Request
::
ACTION_STOP
;
// stahhp
waypointAction
.
request
.
action
=
dji_osdk_ros
::
MissionWpAction
::
Request
::
ACTION_STOP
;
// stahhp
waypoint_action_client
.
call
(
waypointAction
);
waypoint_action_client
.
call
(
waypointAction
);
waypoint
MissionFlag
=
false
;
gps
MissionFlag
=
false
;
}
}
if
(
position
MissionFlag
)
if
(
local
MissionFlag
)
{
{
position
MissionFlag
=
false
;
local
MissionFlag
=
false
;
}
}
return
true
;
return
true
;
}
}
...
@@ -800,21 +865,6 @@ bool goHome()
...
@@ -800,21 +865,6 @@ bool goHome()
return
flightTaskControl
.
response
.
result
;
return
flightTaskControl
.
response
.
result
;
}
}
// void updateMagnitudeScaler(float newValue)
// {
// // remove the last value and insert new one
// calibrationWindowValues.pop_back();
// auto firstIndex = calibrationWindowValues.begin();
// calibrationWindowValues.insert(firstIndex, newValue);
// float sumer = 0.0;
// for (int i = 0; i < calibrationWindowValues.size(); i++)
// {
// sumer += calibrationWindowValues[i];
// }
// poseMagnitudeScaler = sumer/calibrationWindowValues.size();
// }
void
updateAngleScaler
(
float
newValue
)
void
updateAngleScaler
(
float
newValue
)
{
{
// remove the last value and insert new one
// remove the last value and insert new one
...
@@ -826,42 +876,115 @@ void updateAngleScaler(float newValue)
...
@@ -826,42 +876,115 @@ void updateAngleScaler(float newValue)
{
{
sumer
+=
calibrationWindowValues
[
i
];
sumer
+=
calibrationWindowValues
[
i
];
}
}
// check for first iteration, everything else is zero
// if (sumer == newValue)
// {
// // fill up with first value??
// for (int i = 0; i < calibrationWindowValues.size(); i++)
// {
// calibrationWindowValues[i] = newValue;
// }
// }
angleOffset
=
sumer
/
calibrationWindowValues
.
size
();
angleOffset
=
sumer
/
calibrationWindowValues
.
size
();
// angleOffset = calibrationWindowValues[firstIndex]; // just take most recent
}
}
void
calibratePositionReference
(
std
::
vector
<
float
>
requestedPosition
,
std
::
vector
<
float
>
actualPosition
)
// WIP
void
calibratePositionReference
()
{
{
// calculate rotation matrix
std
::
vector
<
float
>
deltaPosition
=
{
1.0
,
0.0
,
0.0
};
poseRotationMatrix
=
rotateAlign
(
requestedPosition
,
actualPosition
);
std
::
vector
<
float
>
startPosition
{
static_cast
<
float
>
(
ex_pos
.
translation
.
x
),
static_cast
<
float
>
(
ex_pos
.
translation
.
y
),
static_cast
<
float
>
(
ex_pos
.
translation
.
z
)
};
// go forward at 1m/s for 1s
relativeVelocity
(
deltaPosition
[
0
],
deltaPosition
[
1
],
deltaPosition
[
2
],
0.0
,
1.0
);
ros
::
Rate
(
1
).
sleep
();
ros
::
spinOnce
();
// let topics update
std
::
vector
<
float
>
deltaPositionActual
{
static_cast
<
float
>
(
ex_pos
.
translation
.
x
)
-
startPosition
[
0
],
static_cast
<
float
>
(
ex_pos
.
translation
.
y
)
-
startPosition
[
1
],
static_cast
<
float
>
(
ex_pos
.
translation
.
z
)
-
startPosition
[
2
]
};
relativeVelocity
(
-
deltaPosition
[
0
],
-
deltaPosition
[
1
],
-
deltaPosition
[
2
],
0.0
,
1.0
);
ros
::
Rate
(
1
).
sleep
();
ros
::
spinOnce
();
// recalculate the rotation offset
// check expected velocity verses actual velocity
ROS_INFO
(
"calibrating angle offset"
);
if
(
Magnitude3
(
deltaPosition
)
!=
0.0
&&
Magnitude3
(
deltaPositionActual
)
!=
0.0
)
{
float
ang
=
-
flatAngleBetween
(
deltaPositionActual
,
deltaPosition
);
ROS_INFO
(
"new angle offset: %f"
,
ang
);
angleOffsetParam
=
static_cast
<
double
>
(
ang
);
// updateAngleScaler(ang);
}
// now get scaling difference between coordinate frames
}
float
requestedPositionMag
=
Magnitude3
(
requestedPosition
);
float
actualPositionMag
=
Magnitude3
(
actualPosition
);
poseMagnitudeScaler
=
(
requestedPositionMag
/
actualPositionMag
);
void
pdMaxSpeedCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update MaxSpeed parameters
maxSpeedParam
=
msg
->
data
;
return
;
}
calibrationWindowValues
=
std
::
vector
<
float
>
{};
void
pdMaxYawSpeedCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
poseRotationMatrixValues
=
std
::
vector
<
std
::
vector
<
std
::
vector
<
float
>>>
{{{}}};
{
for
(
int
i
=
0
;
i
<
calibrationWindow
;
i
++
)
// update MaxYawSpeed parameters
maxYawSpeedParam
=
msg
->
data
;
return
;
}
void
pdSpeedScaleCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update SpeedScale parameters
speedScalarParam
=
msg
->
data
;
return
;
}
void
pdDampingCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update SpeedScale parameters
if
(
msg
->
data
<
0.0
)
{
{
calibrationWindowValues
.
push_back
(
poseMagnitudeScaler
);
return
;
poseRotationMatrixValues
.
push_back
(
poseRotationMatrix
);
}
}
damping
=
static_cast
<
double
>
(
msg
->
data
);
kp
=
(
6.0
*
frequency
)
*
(
6.0
*
frequency
)
*
0.25
;
kd
=
4.5
*
frequency
*
damping
;
return
;
}
ROS_INFO
(
"pose magnitude scalar %f"
,
poseMagnitudeScaler
);
void
pdFrequencyCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update SpeedScale parameters
if
(
msg
->
data
<
0.0
)
{
return
;
}
frequency
=
static_cast
<
double
>
(
msg
->
data
);
kp
=
(
6.0
*
frequency
)
*
(
6.0
*
frequency
)
*
0.25
;
kd
=
4.5
*
frequency
*
damping
;
return
;
}
void
pdAngleOffsetCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update SpeedScale parameters
angleOffsetParam
=
msg
->
data
;
return
;
}
}
// bool add(beginner_tutorials::AddTwoInts::Request &req, beginner_tutorials::AddTwoInts::Response &res)
void
pdUpdateRateCallback
(
const
std_msgs
::
Float32
::
ConstPtr
&
msg
)
{
// update UpdateRate parameters
if
(
msg
->
data
==
0.0
)
{
return
;
}
updateRateParam
=
msg
->
data
;
controlLoopTime
=
1.0
/
updateRateParam
;
return
;
}
bool
overwatchFunction
(
dji_osdk_ros
::
Overwatch
::
Request
&
req
,
dji_osdk_ros
::
Overwatch
::
Response
&
res
)
bool
overwatchFunction
(
dji_osdk_ros
::
Overwatch
::
Request
&
req
,
dji_osdk_ros
::
Overwatch
::
Response
&
res
)
{
{
...
@@ -916,15 +1039,12 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
...
@@ -916,15 +1039,12 @@ bool overwatchFunction(dji_osdk_ros::Overwatch::Request &req, dji_osdk_ros::Over
takeoff
();
takeoff
();
break
;
break
;
}
}
// case 7:
case
7
:
// {
{
// ROS_INFO("received overwatch: calibrate pos ref");
ROS_INFO
(
"received overwatch: calibrate pos ref"
);
//
calibratePositionReference
();
// std::vector<float> requestedPosition = {0.5, 0.0, 0.0};
break
;
// std::vector<float> actualPosition = DoCalibrationMove(requestedPosition);
}
// calibratePositionReference(requestedPosition, actualPosition);
// break;
// }
default
:
default
:
break
;
break
;
}
}
...
@@ -939,13 +1059,14 @@ int main(int argc, char** argv)
...
@@ -939,13 +1059,14 @@ int main(int argc, char** argv)
ros
::
init
(
argc
,
argv
,
"mission_node"
);
ros
::
init
(
argc
,
argv
,
"mission_node"
);
ros
::
NodeHandle
nh
;
ros
::
NodeHandle
nh
;
ros
::
param
::
get
(
"base_speed"
,
baseSpeedParam
);
ros
::
param
::
get
(
"base_speed"
,
waypointSpeedParam
);
ros
::
param
::
get
(
"min_speed"
,
minSpeedParam
);
ros
::
param
::
get
(
"max_speed"
,
maxSpeedParam
);
ros
::
param
::
get
(
"max_speed"
,
maxSpeedParam
);
ros
::
param
::
get
(
"max_yaw_speed"
,
maxYawSpeedParam
);
ros
::
param
::
get
(
"max_yaw_speed"
,
maxYawSpeedParam
);
ros
::
param
::
get
(
"angle_offset"
,
angleOffsetParam
);
ros
::
param
::
get
(
"speed_scalar"
,
speedScalarParam
);
ros
::
param
::
get
(
"speed_scalar"
,
speedScalarParam
);
ros
::
param
::
get
(
"drone_id"
,
droneId
);
ros
::
param
::
get
(
"drone_id"
,
droneId
);
ros
::
param
::
get
(
"position_playback"
,
positionPlaybackParam
);
ros
::
param
::
get
(
"mission_type"
,
missionTypeParam
);
ros
::
param
::
get
(
"position_threshold"
,
positionThresholdParam
);
ros
::
param
::
get
(
"loop_rate"
,
updateRateParam
);
ros
::
param
::
get
(
"loop_rate"
,
updateRateParam
);
controlLoopTime
=
1.0
/
updateRateParam
;
controlLoopTime
=
1.0
/
updateRateParam
;
// additions
// additions
...
@@ -960,29 +1081,38 @@ int main(int argc, char** argv)
...
@@ -960,29 +1081,38 @@ int main(int argc, char** argv)
{
{
calibrationWindowValues
.
push_back
(
0.0
);
calibrationWindowValues
.
push_back
(
0.0
);
}
}
// kp = (18.0*frequency*frequency)/2.0;
// kd = (9.0/2.0)*frequency*damping;
previousWallTime
=
ros
::
WallTime
::
now
();
// set puredata position to zero zero so it doesn't immediately fly away
pd_pos
.
translation
.
x
=
0.0
;
pd_pos
.
translation
.
y
=
0.0
;
pd_pos
.
translation
.
z
=
1.5
;
pd_pos
.
rotation
.
x
=
0.0
;
// ROS stuff
// ROS stuff
// obtain control authority?
// obtain control authority?
obtain_ctrl_authority_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
ObtainControlAuthority
>
(
obtain_ctrl_authority_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
ObtainControlAuthority
>
(
"obtain_release_control_authority"
);
"obtain_release_control_authority"
);
waypoint_upload_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
MissionWpUpload
>
(
waypoint_upload_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
MissionWpUpload
>
(
"dji_osdk_ros/mission_waypoint_upload"
);
"dji_osdk_ros/mission_waypoint_upload"
);
waypoint_action_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
MissionWpAction
>
(
waypoint_action_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
MissionWpAction
>
(
"dji_osdk_ros/mission_waypoint_action"
);
"dji_osdk_ros/mission_waypoint_action"
);
flight_control_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
FlightTaskControl
>
(
flight_control_client
=
nh
.
serviceClient
<
dji_osdk_ros
::
FlightTaskControl
>
(
"flight_task_control"
);
"flight_task_control"
);
syncPub
=
nh
.
advertise
<
std_msgs
::
Float32
>
(
"sync_signal"
,
1
);
//
syncPub = nh.advertise<std_msgs::Float32>("sync_signal", 1);
gps_pos_subscriber
=
nh
.
subscribe
<
sensor_msgs
::
NavSatFix
>
(
"dji_osdk_ros/gps_position"
,
1
,
&
gpsPosCallback
);
gps_pos_subscriber
=
nh
.
subscribe
<
sensor_msgs
::
NavSatFix
>
(
"dji_osdk_ros/gps_position"
,
1
,
&
gpsPosCallback
);
pos_feedback_subscriber
=
nh
.
subscribe
<
geometry_msgs
::
Transform
>
(
"feedback"
,
1
,
&
posFeedbackCallback
);
pos_feedback_subscriber
=
nh
.
subscribe
<
geometry_msgs
::
Transform
>
(
"feedback"
,
1
,
&
posFeedbackCallback
);
pos_tracking_subscriber
=
nh
.
subscribe
<
geometry_msgs
::
Transform
>
(
"tracking"
,
1
,
&
posTrackingCallback
);
pos_tracking_subscriber
=
nh
.
subscribe
<
geometry_msgs
::
Transform
>
(
"tracking"
,
1
,
&
posTrackingCallback
);
pd_position_subscriber
=
nh
.
subscribe
<
geometry_msgs
::
Transform
>
(
"pd/target"
,
1
,
&
pdPositionCallback
);
// pd_parameter_MinSpeed_subscriber = nh.subscribe<std_msgs::Float32>("pd/parameters/minSpeed", 1, &pdMinSpeedCallback);
pd_parameter_MaxSpeed_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/maxSpeed"
,
1
,
&
pdMaxSpeedCallback
);
pd_parameter_MaxYawSpeed_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/maxYawSpeed"
,
1
,
&
pdMaxYawSpeedCallback
);
pd_parameter_SpeedScale_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/speedScale"
,
1
,
&
pdSpeedScaleCallback
);
pd_parameter_UpdateRate_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/updateRate"
,
1
,
&
pdUpdateRateCallback
);
pd_parameter_AngleOffset_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/angleOffset"
,
1
,
&
pdAngleOffsetCallback
);
pd_parameter_Damping_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/controllerDamping"
,
1
,
&
pdDampingCallback
);
pd_parameter_Frequency_subscriber
=
nh
.
subscribe
<
std_msgs
::
Float32
>
(
"pd/parameters/controllerFrequency"
,
1
,
&
pdFrequencyCallback
);
ros
::
ServiceServer
overwatchService
=
nh
.
advertiseService
(
"overwatch"
,
overwatchFunction
);
ros
::
ServiceServer
overwatchService
=
nh
.
advertiseService
(
"overwatch"
,
overwatchFunction
);
...
...
misson_controller/CMakeLists.txt
0 → 100644
View file @
c2e326c8
cmake_minimum_required
(
VERSION 3.0.2
)
project
(
misson_controller
)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package
(
catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rospy
sensor_msgs
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# sensor_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package
(
# INCLUDE_DIRS include
# LIBRARIES misson_controller
# CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories
(
# include
${
catkin_INCLUDE_DIRS
}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/misson_controller.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/misson_controller_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_misson_controller.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
misson_controller/package.xml
0 → 100644
View file @
c2e326c8
<?xml version="1.0"?>
<package
format=
"2"
>
<name>
misson_controller
</name>
<version>
0.0.0
</version>
<description>
The misson_controller package
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer
email=
"hlarkin3@vt.edu"
>
larkin
</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>
TODO
</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/misson_controller</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>
catkin
</buildtool_depend>
<build_depend>
geometry_msgs
</build_depend>
<build_depend>
roscpp
</build_depend>
<build_depend>
rospy
</build_depend>
<build_depend>
sensor_msgs
</build_depend>
<build_depend>
std_msgs
</build_depend>
<build_export_depend>
geometry_msgs
</build_export_depend>
<build_export_depend>
roscpp
</build_export_depend>
<build_export_depend>
rospy
</build_export_depend>
<build_export_depend>
sensor_msgs
</build_export_depend>
<build_export_depend>
std_msgs
</build_export_depend>
<exec_depend>
geometry_msgs
</exec_depend>
<exec_depend>
roscpp
</exec_depend>
<exec_depend>
rospy
</exec_depend>
<exec_depend>
sensor_msgs
</exec_depend>
<exec_depend>
std_msgs
</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
puredata/CMakeLists.txt
0 → 100644
View file @
c2e326c8
cmake_minimum_required
(
VERSION 3.0.2
)
project
(
puredata
)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package
(
catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package
(
INCLUDE_DIRS include
LIBRARIES puredata
CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories
(
# include
${
catkin_INCLUDE_DIRS
}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/puredata.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/puredata_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_puredata.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
puredata/package.xml
0 → 100644
View file @
c2e326c8
<?xml version="1.0"?>
<package
format=
"2"
>
<name>
puredata
</name>
<version>
0.0.0
</version>
<description>
The puredata package
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer
email=
"hlarkin3@vt.edu"
>
larkin
</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>
TODO
</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/puredata</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>
catkin
</buildtool_depend>
<build_depend>
roscpp
</build_depend>
<build_depend>
rospy
</build_depend>
<build_depend>
std_msgs
</build_depend>
<build_depend>
geometry_msgs
</build_depend>
<build_export_depend>
roscpp
</build_export_depend>
<build_export_depend>
rospy
</build_export_depend>
<build_export_depend>
std_msgs
</build_export_depend>
<build_export_depend>
geometry_msgs
</build_export_depend>
<exec_depend>
roscpp
</exec_depend>
<exec_depend>
rospy
</exec_depend>
<exec_depend>
std_msgs
</exec_depend>
<exec_depend>
geometry_msgs
</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
puredata/patches/crushedPiano.wav
0 → 100644
View file @
c2e326c8
File added
puredata/patches/drone-osc.pd
0 → 100644
View file @
c2e326c8
#N canvas 0 96 1013 984 10;
#X obj 54 73 netreceive -u -b;
#X obj 55 98 oscparse;
#X obj 210 23 loadbang;
#X obj 210 56 del 100;
#X obj 55 123 route list;
#X obj 55 159 route sound;
#X obj 121 461 dac~ 1 2;
#X msg 42 279 start;
#X obj 55 211 select 1 0;
#X obj 54 252 t b b;
#X msg 110 273 stop;
#X obj 55 185 route one;
#X obj 121 428 *~;
#X obj 170 427 *~;
#X obj 227 154 route master;
#X obj 215 264 line~;
#X msg 227 182 \$1 100;
#X msg 210 83 \; pd dsp 1;
#X obj 16 202 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144 -1
-1;
#X obj 187 21 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144 -1
-1;
#X obj 95 356 readsf~ 2;
#X obj 175 392 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144
-1 -1;
#X obj 132 160 print oscin;
#X obj 343 387 readsf~ 2;
#X obj 345 429 dac~;
#X obj 317 277 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144
-1 -1;
#X msg 305 356 start;
#X msg 358 362 stop;
#X obj 304 337 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144
-1 -1;
#X obj 355 342 bng 15 250 50 0 empty empty empty 17 7 0 10 -262144
-1 -1;
#X msg 54 49 listen 3722;
#X text 305 261 random testing stuff;
#X floatatom 280 182 5 0 0 0 - - -;
#X obj 53 6 loadbang;
#X text 34 334 need to pick different sounds here;
#X msg 340 305 open ./crushedPiano.wav;
#X obj 54 26 del 500;
#X msg 91 309 open ~/offlinePayload1/payload2-full.aif;
#X connect 0 0 1 0;
#X connect 1 0 4 0;
#X connect 2 0 3 0;
#X connect 3 0 17 0;
#X connect 4 0 5 0;
#X connect 4 0 14 0;
#X connect 4 0 22 0;
#X connect 5 0 11 0;
#X connect 7 0 20 0;
#X connect 8 0 9 0;
#X connect 8 1 10 0;
#X connect 9 0 7 0;
#X connect 9 1 37 0;
#X connect 10 0 20 0;
#X connect 11 0 8 0;
#X connect 12 0 6 0;
#X connect 13 0 6 1;
#X connect 14 0 16 0;
#X connect 14 0 32 0;
#X connect 15 0 12 1;
#X connect 15 0 13 1;
#X connect 16 0 15 0;
#X connect 18 0 9 0;
#X connect 19 0 3 0;
#X connect 20 0 12 0;
#X connect 20 1 13 0;
#X connect 20 2 21 0;
#X connect 23 0 24 0;
#X connect 23 1 24 1;
#X connect 25 0 35 0;
#X connect 26 0 23 0;
#X connect 27 0 23 0;
#X connect 28 0 26 0;
#X connect 29 0 27 0;
#X connect 30 0 0 0;
#X connect 33 0 36 0;
#X connect 35 0 23 0;
#X connect 36 0 30 0;
#X connect 37 0 20 0;
puredata/scripts/puredataLaunch.bash
0 → 100755
View file @
c2e326c8
#!/bin/bash
# need to figure out what these audio numbers should be!!
puredata
-r
48000
-nogui
-audiooutdev
5
-audioindev
5 ../patches/drone-osc.pd
puredata/scripts/puredataPublisher.py
0 → 100755
View file @
c2e326c8
#! /usr/bin/python3
import
rospy
import
sys
# from osc4py3.as_eventloop import *
# from osc4py3.as_allthreads import *
from
osc4py3.as_comthreads
import
*
from
osc4py3
import
oscmethod
as
osm
from
geometry_msgs.msg
import
Transform
from
std_msgs.msg
import
Float32
import
netifaces
as
ni
# blah blah blah
# AHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
class
OSCTranslator
():
def
__init__
(
self
):
self
.
positionPub
=
rospy
.
Publisher
(
'pd/target'
,
Transform
,
queue_size
=
1
)
self
.
paramMaxSpeedPub
=
rospy
.
Publisher
(
'pd/parameters/maxSpeed'
,
Float32
,
queue_size
=
1
)
self
.
paramMaxYawSpeedPub
=
rospy
.
Publisher
(
'pd/parameters/maxYawSpeed'
,
Float32
,
queue_size
=
1
)
self
.
paramSpeedScalePub
=
rospy
.
Publisher
(
'pd/parameters/speedScale'
,
Float32
,
queue_size
=
1
)
self
.
paramUpdateRatePub
=
rospy
.
Publisher
(
'pd/parameters/updateRate'
,
Float32
,
queue_size
=
1
)
self
.
paramDampingPub
=
rospy
.
Publisher
(
'pd/parameters/damping'
,
Float32
,
queue_size
=
1
)
self
.
paramFrequencyPub
=
rospy
.
Publisher
(
'pd/parameters/frequency'
,
Float32
,
queue_size
=
1
)
self
.
paramAngleOffsetPub
=
rospy
.
Publisher
(
'pd/parameters/angleOffset'
,
Float32
,
queue_size
=
1
)
rospy
.
init_node
(
'puredata_publisher'
)
self
.
rate
=
rospy
.
Rate
(
100
)
self
.
target
=
Transform
()
rospy
.
loginfo
(
"starting osc server"
)
# Make server channels to receive packets.
# get wifi interfaces ip
# dont listen on the wireless when wanting it through the ethernet~
# osc_udp_server(ni.ifaddresses('wlan0')[ni.AF_INET][0]['addr'], 3721, "aservername")
osc_udp_server
(
ni
.
ifaddresses
(
'eth0'
)[
ni
.
AF_INET
][
0
][
'addr'
],
3721
,
"aservername"
)
osc_method
(
"/position"
,
self
.
handlerfunctionflex
,
argscheme
=
osm
.
OSCARG_ADDRESS
+
osm
.
OSCARG_DATAUNPACK
)
osc_method
(
"/parameters"
,
self
.
handlerfunctionflex
,
argscheme
=
osm
.
OSCARG_ADDRESS
+
osm
.
OSCARG_DATAUNPACK
)
osc_method
(
"/sound/one"
,
self
.
handlerfunctionflex
,
argscheme
=
osm
.
OSCARG_ADDRESS
+
osm
.
OSCARG_DATAUNPACK
)
osc_method
(
"/master"
,
self
.
handlerfunctionflex
,
argscheme
=
osm
.
OSCARG_ADDRESS
+
osm
.
OSCARG_DATAUNPACK
)
def
handlerfunctionflex
(
self
,
address
,
*
args
):
# Will receive message data unpacked in s, x, y
rospy
.
loginfo
(
"from: "
+
address
+
": "
+
str
(
args
))
if
(
address
==
"/position"
):
# rospy.loginfo("publishing position...")
self
.
target
.
translation
.
x
=
args
[
0
]
self
.
target
.
translation
.
y
=
args
[
1
]
self
.
target
.
translation
.
z
=
args
[
2
]
self
.
target
.
rotation
.
x
=
args
[
3
]
self
.
positionPub
.
publish
(
self
.
target
)
elif
(
address
==
"/parameters/maxSpeed"
):
# rospy.loginfo("new maxSpeed param: " + str(args[0]))
self
.
paramMaxSpeedPub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/maxYawSpeed"
):
# rospy.loginfo("new maxYawSpeed param: " + str(args[0]))
self
.
paramMaxYawSpeedSpeedPub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/speedScale"
):
# rospy.loginfo("new speedScale param: " + str(args[0]))
self
.
paramSpeedScalePub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/updateRate"
):
# rospy.loginfo("new updateRate param: " + str(args[0]))
self
.
paramUpdateRatePub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/damping"
):
# rospy.loginfo("new damping param: " + str(args[0]))
self
.
paramDampingPub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/frequency"
):
# rospy.loginfo("new frequency param: " + str(args[0]))
self
.
paramFrequencyPub
.
publish
(
args
[
0
])
elif
(
address
==
"/parameters/angleOffset"
):
# rospy.loginfo("new angleOffset param: " + str(args[0]))
self
.
paramAngleOffsetPub
.
publish
(
args
[
0
])
elif
(
address
==
"/sound/one"
):
rospy
.
loginfo
(
"publishing sound..."
)
elif
(
address
==
"/master"
):
rospy
.
loginfo
(
"publishing master vol..."
)
# pure data will handle sounds..?
pass
if
__name__
==
"__main__"
:
osc_startup
()
oscer
=
OSCTranslator
()
rospy
.
loginfo
(
"starting osc process loop"
)
while
not
rospy
.
is_shutdown
():
# rospy.loginfo("processing osc msgs...")
osc_process
()
oscer
.
rate
.
sleep
()
# rospy.spin()
osc_terminate
()
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment