Skip to content
This repository was archived by the owner on Jan 23, 2024. It is now read-only.

Feature/time sync #3

Closed
wants to merge 8 commits into from
Closed

Feature/time sync #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
85821d8
update from outdoor flights on 10/03/2017
inkyusa Mar 16, 2017
367c4f8
save current work
inkyusa Mar 19, 2017
77ee67e
add waypoint navigator using DJI api
inkyusa Mar 19, 2017
b9de29d
working waypoint navigation with path file
raghavkhanna Mar 23, 2017
0d5d079
Add translated hardware timestamps to all messages
raghavkhanna Mar 24, 2017
7af01d5
ignore vscode files in git
raghavkhanna Mar 24, 2017
303e2dd
remove unwanted files
raghavkhanna Mar 24, 2017
6faff1b
thrust command should not go below 10
raghavkhanna Mar 27, 2017
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,5 @@
*.swn
*.swo
*.swp
.vscode/
build.sh
1 change: 1 addition & 0 deletions dji_sdk/&
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
export PATH="/usr/lib/ccache:$PATH"
1 change: 1 addition & 0 deletions dji_sdk/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
mav_msgs
keyboard
joy
cuckoo_time_translator
)

## System dependencies are found with CMake's conventions
Expand Down
84 changes: 84 additions & 0 deletions dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_15m_rect.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
<launch>
<arg name="mav_name" default="flourish"/>
<arg name="namespace" default="$(arg mav_name)" />
<rosparam file="$(find dji_sdk)/resources/capabilities.yaml"/>
<group ns="$(arg namespace)" >

<node name="dji_sdk" pkg="dji_sdk" type="dji_sdk_node" output="screen">
<param name="serial_name" type="string" value="/dev/ttyUSB0"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1027253"/>
<param name="app_version" type="int" value="1"/>
<param name="app_bundle_id" type="string" value="Welcome to use dji-sdk"/>
<param name="enc_key" type="string" value="2267d8cc7874b641a2d16798afb659ba4876b5dfd9b0a0e471c00e188b5c9ed3"/>
<param name="groundstation_enable" type="int" value="1"/>
</node>

<node name="pose_sensor_rovio" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen">
<remap from="msf_core/imu_state_input" to="dji_sdk/imu" />
<remap from="msf_updates/transform_input" to="rovio/transform" />
<rosparam file="$(find dji_sdk)/resources/msf_parameters_vision_realsense-2017-02-26.yaml"/>
<param name="capability_group" value="Core" />
</node>

<node name="mav_linear_mpc" pkg="mav_linear_mpc" type="mav_linear_mpc_node" respawn="true" clear_params="true" output="screen">
<remap from="odometry" to="msf_core/odometry" />
<param name="use_rc_teleop" value="false" />
<remap from="command/roll_pitch_yawrate_thrust" to="fcu/command/roll_pitch_yawrate_thrust" />
<rosparam file="$(find mav_linear_mpc)/resources/linear_mpc_rovio_$(arg mav_name).yaml"/>
<rosparam file="$(find mav_disturbance_observer)/resources/disturbance_observer_$(arg mav_name).yaml"/>
<param name="capability_group" value="Core" />
</node>

<node pkg="realsense_ros" type="zr300node" name="realsense_zr300">
<param name="imu/intrinsics_yaml" value="$(find dji_sdk)/resources/sensors/intel2/realsense.yaml"/>
<param name="imu/publish_sensors_inidividually" value="false"/>

<param name="depth/width" value="640"/>
<param name="depth/height" value="480"/>
<param name="depth/fps" value="30"/>
<param name="depth/enabled" value="false"/>
<param name="depth/subsample_factor" value="3"/>

<param name="infrared/enabled" value="false"/>

<param name="color/width" value="640"/>
<param name="color/height" value="480"/>
<param name="color/fps" value="30"/>
<param name="color/enabled" value="false"/>
<param name="color/subsample_factor" value="3"/>

<param name="fisheye/width" value="640"/>
<param name="fisheye/height" value="480"/>
<param name="fisheye/fps" value="30"/>
<param name="fisheye/enabled" value="true"/>
<param name="fisheye/subsample_factor" value="1"/>
<param name="pointcloud/enabled" value="false"/>
</node>

<node name="rovio" pkg="rovio" type="rovio_node" output="screen">
<param name="filter_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_filter.info" />
<param name="camera0_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_cam.yaml" />
<remap from="cam0/image_raw" to="fisheye/fisheye"/>
<remap from="imu0" to="imu/compensated"/>
<param name="world_frame" value="odom"/>
<param name="capability_group" value="Rovio" />
</node>
<!-- Init Rovio Yaw-->
<node pkg="rovio" type="init_rovio_enu.py" name="init_rovio_enu" output="screen">
<remap from="mag_imu" to="dji_sdk/imu" />
</node>

<node name="waypoint_navigator_node" pkg="waypoint_navigator" type="waypoint_navigator_node" respawn="true" clear_params="true" output="screen">
<rosparam file="$(find dji_sdk)/resources/trajectory_exp_m100_rect_15m.yaml"/>
<param name="mav_name" type="string" value="$(arg namespace)" />
<!-- Real -->
<!-- remap from="odometry" to="msf_core/odometry" / -->
<!-- Simulation -->
<!-- <remap from="odometry" to="ground_truth/odometry" /> -->
<remap from="odometry" to="msf_core/odometry" />
</node>
<node name="trajectory_sampling" pkg="mav_planning_utils" type="trajectory_sampling_node" output="screen" />

</group>
</launch>
85 changes: 85 additions & 0 deletions dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_1m_rect.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
<launch>
<arg name="mav_name" default="flourish"/>
<arg name="namespace" default="$(arg mav_name)" />
<rosparam file="$(find dji_sdk)/resources/capabilities.yaml"/>
<group ns="$(arg namespace)" >

<node name="dji_sdk" pkg="dji_sdk" type="dji_sdk_node" output="screen">
<param name="serial_name" type="string" value="/dev/ttyUSB0"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1027253"/>
<param name="app_version" type="int" value="1"/>
<param name="app_bundle_id" type="string" value="Welcome to use dji-sdk"/>
<param name="enc_key" type="string" value="2267d8cc7874b641a2d16798afb659ba4876b5dfd9b0a0e471c00e188b5c9ed3"/>
<param name="groundstation_enable" type="int" value="1"/>
</node>

<node name="pose_sensor_rovio" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen">
<remap from="msf_core/imu_state_input" to="dji_sdk/imu" />
<remap from="msf_updates/transform_input" to="rovio/transform" />
<rosparam file="$(find dji_sdk)/resources/msf_parameters_vision_realsense-2017-02-26.yaml"/>
<param name="capability_group" value="Core" />
</node>

<node name="mav_linear_mpc" pkg="mav_linear_mpc" type="mav_linear_mpc_node" respawn="true" clear_params="true" output="screen">
<remap from="odometry" to="msf_core/odometry" />
<param name="use_rc_teleop" value="false" />
<remap from="command/roll_pitch_yawrate_thrust" to="fcu/command/roll_pitch_yawrate_thrust" />
<rosparam file="$(find mav_linear_mpc)/resources/linear_mpc_rovio_$(arg mav_name).yaml"/>
<rosparam file="$(find mav_disturbance_observer)/resources/disturbance_observer_$(arg mav_name).yaml"/>
<param name="capability_group" value="Core" />
</node>

<node pkg="realsense_ros" type="zr300node" name="realsense_zr300">
<param name="imu/intrinsics_yaml" value="$(find dji_sdk)/resources/sensors/intel2/realsense.yaml"/>
<param name="imu/publish_sensors_inidividually" value="false"/>

<param name="depth/width" value="640"/>
<param name="depth/height" value="480"/>
<param name="depth/fps" value="30"/>
<param name="depth/enabled" value="false"/>
<param name="depth/subsample_factor" value="3"/>

<param name="infrared/enabled" value="false"/>

<param name="color/width" value="640"/>
<param name="color/height" value="480"/>
<param name="color/fps" value="30"/>
<param name="color/enabled" value="false"/>
<param name="color/subsample_factor" value="3"/>

<param name="fisheye/width" value="640"/>
<param name="fisheye/height" value="480"/>
<param name="fisheye/fps" value="30"/>
<param name="fisheye/enabled" value="true"/>
<param name="fisheye/subsample_factor" value="1"/>
<param name="pointcloud/enabled" value="false"/>
</node>

<node name="rovio" pkg="rovio" type="rovio_node" output="screen">
<param name="filter_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_filter.info" />
<param name="camera0_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_cam.yaml" />
<remap from="cam0/image_raw" to="fisheye/fisheye"/>
<remap from="imu0" to="imu/compensated"/>
<param name="world_frame" value="odom"/>
<param name="capability_group" value="Rovio" />
</node>

<!-- Init Rovio Yaw-->
<node pkg="rovio" type="init_rovio_enu.py" name="init_rovio_enu" output="screen">
<remap from="mag_imu" to="dji_sdk/imu" />
</node>

<node name="waypoint_navigator_node" pkg="waypoint_navigator" type="waypoint_navigator_node" respawn="true" clear_params="true" output="screen">
<rosparam file="$(find dji_sdk)/resources/trajectory_exp_m100_rect_1m.yaml"/>
<param name="mav_name" type="string" value="$(arg namespace)" />
<!-- Real -->
<!-- remap from="odometry" to="msf_core/odometry" / -->
<!-- Simulation -->
<!-- <remap from="odometry" to="ground_truth/odometry" /> -->
<remap from="odometry" to="msf_core/odometry" />
</node>
<node name="trajectory_sampling" pkg="mav_planning_utils" type="trajectory_sampling_node" output="screen" />

</group>
</launch>
89 changes: 89 additions & 0 deletions dji_sdk/asl_launch/SimulateM100.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,89 @@
<launch>
<!--arg name="mav_name" default="flourish"/>
<arg name="namespace" default="$(arg mav_name)" />
<rosparam file="$(find dji_sdk)/resources/capabilities.yaml"/>
<group ns="$(arg namespace)" -->

<node name="dji_sdk" pkg="dji_sdk" type="dji_sdk_node" output="screen">
<param name="serial_name" type="string" value="/dev/autopilot"/>
<param name="baud_rate" type="int" value="921600"/>
<param name="app_id" type="int" value="1027253"/>
<param name="app_version" type="int" value="1"/>
<param name="app_bundle_id" type="string" value="Welcome to use dji-sdk"/>
<param name="enc_key" type="string" value="2267d8cc7874b641a2d16798afb659ba4876b5dfd9b0a0e471c00e188b5c9ed3"/>
<param name="groundstation_enable" type="int" value="1"/>
</node>

<!--node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
<rosparam command="load" file="$(find robot_localization)/params/navsat_transform_template.yaml" /-->

<!-- Placeholders for input remapping. Set your topic names as the "to" values.
<remap from="imu/data" to="/dji_sdk/imu"/>
<remap from="odometry/filtered" to="/dji_sdk/odometry"/>
<remap from="gps/fix" to="/dji_sdk/gps"/>
</node>
-->


<!--node name="mav_linear_mpc" pkg="mav_linear_mpc" type="mav_linear_mpc_node" respawn="true" clear_params="true" output="screen">
<remap from="odometry" to="msf_core/odometry" />
<param name="use_rc_teleop" value="false" />
<remap from="command/roll_pitch_yawrate_thrust" to="fcu/command/roll_pitch_yawrate_thrust" />
<rosparam file="$(find mav_linear_mpc)/resources/linear_mpc_rovio_$(arg mav_name).yaml"/>
<rosparam file="$(find mav_disturbance_observer)/resources/disturbance_observer_$(arg mav_name).yaml"/>
<param name="capability_group" value="Core" />
</node-->

<!--node pkg="realsense_ros" type="zr300node" name="realsense_zr300">
<param name="imu/intrinsics_yaml" value="$(find dji_sdk)/resources/sensors/intel2/realsense.yaml"/>
<param name="imu/publish_sensors_inidividually" value="false"/>

<param name="depth/width" value="640"/>
<param name="depth/height" value="480"/>
<param name="depth/fps" value="30"/>
<param name="depth/enabled" value="false"/>
<param name="depth/subsample_factor" value="3"/>

<param name="infrared/enabled" value="false"/>

<param name="color/width" value="640"/>
<param name="color/height" value="480"/>
<param name="color/fps" value="30"/>
<param name="color/enabled" value="false"/>
<param name="color/subsample_factor" value="3"/>

<param name="fisheye/width" value="640"/>
<param name="fisheye/height" value="480"/>
<param name="fisheye/fps" value="30"/>
<param name="fisheye/enabled" value="true"/>
<param name="fisheye/subsample_factor" value="1"/>
<param name="pointcloud/enabled" value="false"/>
</node-->

<!--node name="rovio" pkg="rovio" type="rovio_node" output="screen">
<param name="filter_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_filter.info" />
<param name="camera0_config" value="$(find dji_sdk)/resources/sensors/intel2/rovio_cam.yaml" />
<remap from="cam0/image_raw" to="fisheye/fisheye"/>
<remap from="imu0" to="imu/compensated"/>
<param name="world_frame" value="odom"/>
<param name="capability_group" value="Rovio" />
</node-->

<!-- Init Rovio Yaw-->
<!--node pkg="rovio" type="init_rovio_enu.py" name="init_rovio_enu" output="screen">
<remap from="mag_imu" to="dji_sdk/imu" />
</node-->

<!--node name="waypoint_navigator_node" pkg="waypoint_navigator" type="waypoint_navigator_node" respawn="true" clear_params="true" output="screen">
<rosparam file="$(find dji_sdk)/resources/trajectory_exp_m100_rect_1m.yaml"/>
<param name="mav_name" type="string" value="$(arg namespace)" /-->
<!-- Real -->
<!-- remap from="odometry" to="msf_core/odometry" / -->
<!-- Simulation -->
<!-- <remap from="odometry" to="ground_truth/odometry" /> -->
<!--remap from="odometry" to="msf_core/odometry" />
</node-->
<!--node name="trajectory_sampling" pkg="mav_planning_utils" type="trajectory_sampling_node" output="screen" /-->

<!--/group-->
</launch>
9 changes: 8 additions & 1 deletion dji_sdk/include/dji_sdk/dji_sdk_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,20 +18,22 @@
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <boost/bind.hpp>
#include <dji_sdk/dji_sdk.h>
#include <actionlib/server/simple_action_server.h>
#include <dji_sdk/dji_sdk_mission.h>
#include <mav_msgs/RollPitchYawrateThrust.h>
#include "keyboard/Key.h"
#include <sensor_msgs/Joy.h>
#include <cuckoo_time_translator/DeviceTimeTranslator.h>

#define C_EARTH (double) 6378137.0
#define C_PI (double) 3.141592653589793
#define DEG2RAD(DEG) ((DEG)*((C_PI)/(180.0)))

extern DJI::onboardSDK::ROSAdapter *rosAdapter;
using namespace DJI::onboardSDK;
using namespace DJI::onboardSDK; //TODO:remove namespaces from headers

class DJISDKNode
{
Expand All @@ -54,8 +56,11 @@ class DJISDKNode
nav_msgs::Odometry odometry;
dji_sdk::TimeStamp time_stamp;
sensor_msgs::Imu imu_msg;
sensor_msgs::NavSatFix gps_msg;
dji_sdk::A3GPS A3_GPS;
dji_sdk::A3RTK A3_RTK;
// Declare Time translator
boost::shared_ptr<cuckoo_time_translator::DeviceTimeUnwrapperAndTranslator> device_time_translator_; // TODO: Why does this have to be a boost::shared_ptr ?
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The boost::shared_ptr is not needed. In the example driver this was the simplest available solution (pre c++11) to the deferred initialization requirement that came with the fact that the node handle was it requires was created in the onInit method.
As your class gets the node handle from outside passed to the constructor you don't need a deferred initialization at all and could make this a simple member and move its initialization to the initialization list of the constructor.



bool activation_result = false;
Expand Down Expand Up @@ -89,6 +94,7 @@ class DJISDKNode

// Add ROS compatible message publishers
ros::Publisher imu_msg_publisher;
ros::Publisher gps_msg_publisher;

ros::Subscriber external_transform_subscriber;
ros::Subscriber cmd_sub_;
Expand Down Expand Up @@ -144,6 +150,7 @@ class DJISDKNode
time_stamp_publisher = nh.advertise<dji_sdk::TimeStamp>("dji_sdk/time_stamp", 10);
data_received_from_remote_device_publisher = nh.advertise<dji_sdk::TransparentTransmissionData>("dji_sdk/data_received_from_remote_device",10);
imu_msg_publisher = nh.advertise<sensor_msgs::Imu>("dji_sdk/imu", 1);
gps_msg_publisher = nh.advertise<sensor_msgs::NavSatFix>("dji_sdk/gps", 1);
vc_cmd_pub_=nh.advertise<mav_msgs::RollPitchYawrateThrust>("/flourish/vc_cmd",1);
//TODO: Identify the drone version first
A3_GPS_info_publisher = nh.advertise<dji_sdk::A3GPS>("dji_sdk/A3_GPS", 10);
Expand Down
2 changes: 2 additions & 0 deletions dji_sdk/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@
<build_depend>mav_msgs</build_depend>
<build_depend>keyboard</build_depend>
<build_depend>joy</build_depend>
<build_depend>cuckoo_time_translator</build_depend>

<run_depend>message_runtime</run_depend>
<run_depend>actionlib</run_depend>
Expand All @@ -68,6 +69,7 @@
<run_depend>keyboard</run_depend>
<run_depend>mav_msgs</run_depend>
<run_depend>joy</run_depend>
<run_depend>cuckoo_time_translator</run_depend>


<!-- The export tag contains other, unspecified, tags -->
Expand Down
Loading