diff --git a/src/beacon_positioning/src/tracker_position.cpp b/src/beacon_positioning/src/tracker_position.cpp index 0b4d9f52..b87c63c3 100644 --- a/src/beacon_positioning/src/tracker_position.cpp +++ b/src/beacon_positioning/src/tracker_position.cpp @@ -19,10 +19,10 @@ using SerialInterface = terabee::serial_communication::Serial; using namespace std::chrono_literals; -class BeaconPositioningPublisher : public rclcpp::Node +class TrackerPositioning : public rclcpp::Node { public: - BeaconPositioningPublisher() : Node("beacon_positioning") + TrackerPositioning() : Node("beacon_positioning") { this->declare_parameter("tracker_serial_port", "/dev/ttyUSB0"); @@ -31,7 +31,7 @@ public: RCLCPP_INFO(this->get_logger(), "serial port is %s\n", serial_port_name.c_str()); - serial_port = std::make_shared("/dev/ttyUSB0"); + serial_port = std::make_shared(serial_port_name); serial_port->setBaudrate(115200); serial_port->setTimeout(800ms); @@ -47,8 +47,6 @@ public: } publisher_ = this->create_publisher("beacon_positioning", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&BeaconPositioningPublisher::timer_callback, this)); } void setup_rtlsdevice(terabee::RtlsDevice *rtls_device, int priority, int label, int update_time, int network_id, bool long_message) @@ -99,16 +97,6 @@ public: } private: - void timer_callback() - - { - // auto message = std_msgs::msg::String(); - // message.data = "Hello beacons!"; - // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - // publisher_->publish(message); - } - - rclcpp::TimerBase::SharedPtr timer_; // timer to trigger the rclcpp::Publisher::SharedPtr publisher_; // pointer to publisher object int tracker_id; @@ -117,7 +105,6 @@ private: terabee::RtlsDevice::config_t device_configuration; terabee::RtlsDevice::OnTrackerDataCallback tracker_data_callback_; terabee::RtlsDevice::tracker_msg_t tracker_msg; - //terabee::RtlsDevice rtls_device; }; int main(int argc, char **argv) @@ -129,7 +116,7 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); rclcpp::executors::MultiThreadedExecutor executor; - std::shared_ptr node = std::make_shared(); + std::shared_ptr node = std::make_shared(); executor.add_node(node); terabee::RtlsDevice rtls_device(node->get_serial_port()); if (node->get_parameter("tracker_serial_port").as_string().compare(TRACKER_0_PORT)) @@ -170,7 +157,6 @@ int main(int argc, char **argv) else { RCLCPP_ERROR(node->get_logger(), "invalid tracker position"); - // RCLCPP_INFO(node->get_logger(), "x = %f, y = %f, z = %f", tracker_msg.tracker_position_xyz[0], tracker_msg.tracker_position_xyz[1], tracker_msg.tracker_position_xyz[2]); } }); rtls_device.startReadingStream(); diff --git a/src/camera/camera/camera_controller.py b/src/camera/camera/camera_controller.py index e589c9e9..9fe9f6c6 100644 --- a/src/camera/camera/camera_controller.py +++ b/src/camera/camera/camera_controller.py @@ -1,16 +1,13 @@ import rclpy from rclpy.node import Node - from drone_services.srv import TakePicture import os from datetime import datetime -# import cv2 - +#resolution of the camera RES_4K_H = 3496 RES_4K_W = 4656 - class CameraController(Node): def __init__(self): super().__init__('camera_controller') @@ -32,19 +29,14 @@ class CameraController(Node): return response - def main(args=None): rclpy.init(args=args) test_controller = CameraController() rclpy.spin(test_controller) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) test_controller.destroy_node() rclpy.shutdown() - if __name__ == '__main__': main() diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp index ed90e095..7d975056 100644 --- a/src/px4_connection/src/px4_controller.cpp +++ b/src/px4_connection/src/px4_controller.cpp @@ -288,7 +288,7 @@ private: // set message to enable attitude auto msg = px4_msgs::msg::VehicleAttitudeSetpoint(); - // move up? + // move up msg.thrust_body[0] = 0; // north msg.thrust_body[1] = 0; // east msg.thrust_body[2] = -last_thrust;