Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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 bebop_driver/config/defaults.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ states:
enable_autotakeoffmodechanged: true
enable_mediastreamingstate_videoenablechanged: true
enable_camerastate_orientation: true
enable_camerastate_orientationv2: true
enable_gpsstate_numberofsatellitechanged: true
enable_numberofsatellitechanged: true

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -1605,6 +1605,9 @@ class Ardrone3CameraStateOrientationV2 : public AbstractState
{
ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str());
ros_pub_ = nh.advertise<bebop_msgs::Ardrone3CameraStateOrientationV2>(topic, 10, true);
::boost::lock_guard<boost::mutex> lock(mutex_);
msg_ptr.reset(new ::bebop_msgs::Ardrone3CameraStateOrientationV2());
ros_pub_.publish(msg_ptr);
} // pub_enabled_ is false
}

Expand Down
2 changes: 1 addition & 1 deletion bebop_driver/launch/bebop_node.launch
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<launch>
<arg name="namespace" default="bebop" />
<arg name="ip" default="192.168.42.1" />
<arg name="drone_type" default="bebop1" /> <!-- available drone types: bebop1, bebop2 -->
<arg name="drone_type" default="bebop2" /> <!-- available drone types: bebop1, bebop2 -->
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/$(arg drone_type)_camera_calib.yaml" />
<group ns="$(arg namespace)">
Expand Down
2 changes: 1 addition & 1 deletion bebop_driver/launch/bebop_nodelet.launch
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<launch>
<arg name="namespace" default="bebop" />
<arg name="ip" default="192.168.42.1" />
<arg name="drone_type" default="bebop1" /> <!-- available drone types: bebop1, bebop2 -->
<arg name="drone_type" default="bebop2" /> <!-- available drone types: bebop1, bebop2 -->
<arg name="config_file" default="$(find bebop_driver)/config/defaults.yaml" />
<arg name="camera_info_url" default="package://bebop_driver/data/$(arg drone_type)_camera_calib.yaml" />
<group ns="$(arg namespace)">
Expand Down
6 changes: 3 additions & 3 deletions bebop_driver/src/bebop.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -553,10 +553,10 @@ void Bebop::Move(const double &roll, const double &pitch, const double &gaz_spee
void Bebop::MoveCamera(const double &tilt, const double &pan)
{
ThrowOnInternalError("Camera Move Failure");
ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientation(
ThrowOnCtrlError(device_controller_ptr_->aRDrone3->setCameraOrientationV2(
device_controller_ptr_->aRDrone3,
static_cast<int8_t>(tilt),
static_cast<int8_t>(pan)));
static_cast<float>(tilt),
static_cast<float>(pan)));
}

uint32_t Bebop::GetFrontCameraFrameWidth() const
Expand Down
7 changes: 4 additions & 3 deletions bebop_driver/src/bebop_driver_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -482,7 +482,8 @@ void BebopDriverNodelet::AuxThread()
util::ResetTwist(zero_twist);

// Camera Pan/Tilt State
bebop_msgs::Ardrone3CameraStateOrientation::ConstPtr camera_state_ptr;
bebop_msgs::Ardrone3CameraStateOrientationV2::ConstPtr camera_state_ptr;
bebop_ptr_->MoveCamera(0.0, 0.0);

// East-South-Down
bebop_msgs::Ardrone3PilotingStateSpeedChanged::ConstPtr speed_esd_ptr;
Expand Down Expand Up @@ -568,9 +569,9 @@ void BebopDriverNodelet::AuxThread()
}
}

if (bebop_ptr_->ardrone3_camerastate_orientation_ptr)
if (bebop_ptr_->ardrone3_camerastate_orientationv2_ptr)
{
camera_state_ptr = bebop_ptr_->ardrone3_camerastate_orientation_ptr->GetDataCstPtr();
camera_state_ptr = bebop_ptr_->ardrone3_camerastate_orientationv2_ptr->GetDataCstPtr();

if ((camera_state_ptr->header.stamp - last_camerastate_time).toSec() > util::eps)
{
Expand Down