Skip to content

Commit 0bb3c8b

Browse files
MikeK4yMikeK4y
committed
post field trial
1 parent c4e6e76 commit 0bb3c8b

File tree

1 file changed

+294
-0
lines changed

1 file changed

+294
-0
lines changed

‎src/dji_gimbal.cpp.orig

+294
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,294 @@
1+
/**
2+
MIT License
3+
4+
Copyright (c) 2019 Michail Kalaitzakis (Unmanned Systems and Robotics Lab,
5+
University of South Carolina, USA)
6+
7+
Permission is hereby granted, free of charge, to any person obtaining a copy
8+
of this software and associated documentation files (the "Software"), to deal
9+
in the Software without restriction, including without limitation the rights
10+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11+
copies of the Software, and to permit persons to whom the Software is
12+
furnished to do so, subject to the following conditions:
13+
14+
The above copyright notice and this permission notice shall be included in all
15+
copies or substantial portions of the Software.
16+
17+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23+
SOFTWARE.
24+
*/
25+
26+
#include "dji_gimbal_cam/dji_gimbal.h"
27+
28+
#include <unistd.h>
29+
30+
#include <iostream>
31+
32+
dji_gimbal::dji_gimbal(ros::NodeHandle& nh)
33+
{
34+
// Configure the gimbal control parameters
35+
initializeParam();
36+
37+
// Setup Subscribers
38+
gimbalAngleSub = nh.subscribe<geometry_msgs::Vector3Stamped>("/dji_sdk/gimbal_angle", 1, &dji_gimbal::gimbalAngleCallback, this);
39+
joySub = nh.subscribe("joy", 1, &dji_gimbal::joyCallback, this);
40+
cameraInfoSub = nh.subscribe(cameraInfoTopic, 1, &dji_gimbal::cameraInfoCallback, this);
41+
pointSub = nh.subscribe(pointTopic, 1, &dji_gimbal::pointCallback, this);
42+
<<<<<<< HEAD
43+
gimbalAngleCMDSub = nh.subscribe(gimbalCmdTopic, 1, &dji_gimbal::gimbalAngleCMDCallback, this);
44+
=======
45+
gimbalAngleCMDSub = nh.sudscribe(gimbalCmdTopic, 1, &dji_gimbal::gimbalAngleCMDCallback, this);
46+
>>>>>>> 140a7fe1c6ae0480ca2e2ac0fa38e747495d35c6
47+
48+
// Setup Publishers
49+
gimbalSpeedPub = nh.advertise<geometry_msgs::Vector3Stamped>("/dji_sdk/gimbal_speed_cmd", 10);
50+
gimbalAnglePub = nh.advertise<dji_sdk::Gimbal>("/dji_sdk/gimbal_angle_cmd", 10);
51+
52+
// Setup Services
53+
facedownServ = nh.advertiseService("facedown", &dji_gimbal::facedownCallback, this);
54+
faceupServ = nh.advertiseService("faceup", &dji_gimbal::faceupCallback, this);
55+
setTrackingServ = nh.advertiseService("setGimbalTracking", &dji_gimbal::setTrackingCallback, this);
56+
}
57+
58+
void dji_gimbal::initializeParam()
59+
{
60+
// Create private nodeHandle to read the launch file
61+
ros::NodeHandle nh_local("");
62+
63+
// Load values from launch file
64+
nh_local.param("track_point", trackPoint, false);
65+
nh_local.param("camera_info_topic", cameraInfoTopic, std::string("/dji_sdk/camera_info"));
66+
nh_local.param("track_point_topic", pointTopic, std::string("track_point"));
67+
nh_local.param("gimbal_cmd_topic", gimbalCmdTopic, std::string("gimbal_cmd"));
68+
nh_local.param("yaw_axis", yawAxis, 0);
69+
nh_local.param("pitch_axis", pitchAxis, 4);
70+
nh_local.param("roll_axis", rollAxis, 3);
71+
nh_local.param("reset_angle_btn", resetButton, 0);
72+
nh_local.param("face_down_btn", faceDownButton, 2);
73+
nh_local.param("toggle_track_btn", toggleButton, 3);
74+
nh_local.param("Kp", Kp, 0.0);
75+
nh_local.param("Kd", Kd, 0.0);
76+
nh_local.param("velLimit", velT, 0.75);
77+
78+
// Initialize speed command
79+
speedCmd.vector.x = 0;
80+
speedCmd.vector.y = 0;
81+
speedCmd.vector.z = 0;
82+
83+
// Initialize flags
84+
pointAvailable = false;
85+
angleAvailable = false;
86+
}
87+
88+
void dji_gimbal::publishGimbalCmd()
89+
{
90+
if (trackPoint)
91+
{
92+
if (pointAvailable)
93+
{
94+
speedCmd.vector.x = 0;
95+
96+
// Corrections in x,y
97+
double cx = posX * Kp + lastVX * Kd;
98+
double cy = -posY * Kp - lastVY * Kd;
99+
100+
// Crop if outside range
101+
cx = cx > velT ? velT : (cx < -velT ? -velT : cx);
102+
cy = cy > velT ? velT : (cy < -velT ? -velT : cy);
103+
104+
// Add to vector
105+
speedCmd.vector.y = cy;
106+
speedCmd.vector.z = cx;
107+
108+
gimbalSpeedPub.publish(speedCmd);
109+
110+
// Reset Commands to zero
111+
speedCmd.vector.y = 0;
112+
speedCmd.vector.z = 0;
113+
114+
<<<<<<< HEAD
115+
116+
}
117+
else if (angleAvailable)
118+
{
119+
// [-pi,pi)
120+
double cr = rollCMD >= M_PI ? rollCMD-2*M_PI : (rollCMD < -M_PI ? rollCMD+2*M_PI : rollCMD);
121+
double cp = pitchCMD >= M_PI ? pitchCMD-2*M_PI : (pitchCMD < -M_PI ? pitchCMD+2*M_PI : pitchCMD);
122+
double cy = yawCMD >= M_PI ? yawCMD-2*M_PI : (yawCMD < -M_PI ? yawCMD+2*M_PI : yawCMD);
123+
124+
double dr = cr - DEG2RAD(gimbalAngle.vector.x);
125+
double dp = cp - DEG2RAD(gimbalAngle.vector.y);
126+
double dy = cy - DEG2RAD(gimbalAngle.vector.z);
127+
speedCmd.vector.x = dr;
128+
speedCmd.vector.y = dp;
129+
speedCmd.vector.z = dy;
130+
131+
gimbalSpeedPub.publish(speedCmd);
132+
133+
// Reset Commands to zero
134+
speedCmd.vector.x = 0;
135+
speedCmd.vector.y = 0;
136+
speedCmd.vector.z = 0;
137+
138+
}
139+
else
140+
gimbalSpeedPub.publish(speedCmd);
141+
}
142+
143+
angleAvailable = pointAvailable = false;
144+
=======
145+
146+
}
147+
else if (angleAvailable)
148+
setGimbalAngle(rollCMD, pitchCMD, yawCMD);
149+
else
150+
gimbalSpeedPub.publish(speedCmd);
151+
}
152+
pointAvailable = false;
153+
angleAvailable = false;
154+
>>>>>>> 140a7fe1c6ae0480ca2e2ac0fa38e747495d35c6
155+
}
156+
157+
void dji_gimbal::setGimbalAngle(double roll, double pitch, double yaw)
158+
{
159+
// Prepare the command
160+
dji_sdk::Gimbal angleCmd;
161+
angleCmd.mode |= 0;
162+
angleCmd.mode |= 1; // for absolute angle
163+
angleCmd.ts = 2;
164+
angleCmd.roll = roll;
165+
angleCmd.pitch = pitch;
166+
angleCmd.yaw = yaw;
167+
168+
gimbalAnglePub.publish(angleCmd);
169+
}
170+
171+
void dji_gimbal::resetGimbalAngle()
172+
{
173+
setGimbalAngle(0, 0, 0);
174+
sleep(2);
175+
}
176+
177+
void dji_gimbal::faceDownwards()
178+
{
179+
// Prepare the angle command
180+
dji_sdk::Gimbal angleCmd;
181+
angleCmd.mode |= 0;
182+
angleCmd.mode |= 1; // for absolute angle
183+
angleCmd.ts = 2;
184+
angleCmd.roll = 0;
185+
angleCmd.pitch = DEG2RAD(-90);
186+
angleCmd.yaw = 0;
187+
188+
gimbalAnglePub.publish(angleCmd);
189+
}
190+
191+
// Callbacks
192+
void dji_gimbal::gimbalAngleCallback(const geometry_msgs::Vector3Stamped::ConstPtr& msg) {
193+
gimbalAngle = *msg;
194+
}
195+
196+
void dji_gimbal::gimbalAngleCMDCallback(const geometry_msgs::Vector3::ConstPtr& msg)
197+
{
198+
rollCMD = msg->x;
199+
pitchCMD = msg->y;
200+
yawCMD = msg->z;
201+
202+
angleAvailable = true;
203+
}
204+
205+
void dji_gimbal::joyCallback(const sensor_msgs::Joy::ConstPtr& msg)
206+
{
207+
// Toggle track flag
208+
if (msg->buttons[toggleButton] == 1)
209+
trackPoint = !trackPoint;
210+
if (msg->buttons[resetButton] == 1)
211+
resetGimbalAngle();
212+
else if (msg->buttons[faceDownButton] == 1)
213+
faceDownwards();
214+
else
215+
{
216+
// Update speed command
217+
speedCmd.vector.x = -msg->axes[rollAxis];
218+
speedCmd.vector.y = -msg->axes[pitchAxis];
219+
speedCmd.vector.z = -msg->axes[yawAxis];
220+
}
221+
}
222+
223+
void dji_gimbal::cameraInfoCallback(const sensor_msgs::CameraInfo& msg)
224+
{
225+
// Get the focal length of the camera
226+
fx = msg.K[0];
227+
fy = msg.K[4];
228+
}
229+
230+
void dji_gimbal::pointCallback(const geometry_msgs::Vector3::ConstPtr& msg)
231+
{
232+
posX = fx * (msg->x / msg->z);
233+
posY = fy * (msg->y / msg->z);
234+
235+
if(lastT != 0)
236+
{
237+
double vx = (posX - lastX) / (ros::Time::now().toSec() - lastT);
238+
double vy = (posY - lastY) / (ros::Time::now().toSec() - lastT);
239+
240+
lastVX = .25 * vx + .75 * lastVX;
241+
lastVY = .25 * vy + .75 * lastVY;
242+
}
243+
244+
lastX = posX;
245+
lastY = posY;
246+
lastT = ros::Time::now().toSec();
247+
248+
pointAvailable = true;
249+
}
250+
251+
bool dji_gimbal::facedownCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
252+
{
253+
faceDownwards();
254+
res.success = true;
255+
return true;
256+
}
257+
258+
bool dji_gimbal::faceupCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
259+
{
260+
resetGimbalAngle();
261+
res.success = true;
262+
return true;
263+
}
264+
265+
bool dji_gimbal::setTrackingCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
266+
{
267+
trackPoint = req.data;
268+
res.success = true;
269+
return true;
270+
}
271+
272+
////////////////////////////////////////////////////////////
273+
//////////////////////// Main ////////////////////////////
274+
////////////////////////////////////////////////////////////
275+
276+
int main(int argc, char** argv)
277+
{
278+
ros::init(argc, argv, "dji_gimbal_control_node");
279+
ros::NodeHandle nh;
280+
281+
dji_gimbal gimbalControl(nh);
282+
283+
ros::Rate rate(10);
284+
285+
while(ros::ok())
286+
{
287+
ros::spinOnce();
288+
gimbalControl.publishGimbalCmd();
289+
290+
rate.sleep();
291+
}
292+
293+
return 0;
294+
}

0 commit comments

Comments
 (0)