Skip to content

Commit

Permalink
imu node
Browse files Browse the repository at this point in the history
  • Loading branch information
davidcalderon03 committed Oct 25, 2024
1 parent bf175b8 commit 0c2c8e9
Show file tree
Hide file tree
Showing 6 changed files with 89 additions and 1 deletion.
1 change: 1 addition & 0 deletions external/vectornav
Submodule vectornav added at 2859fe
14 changes: 13 additions & 1 deletion urc_bringup/launch/bringup.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,16 @@ def generate_launch_description():
arguments=["-p", controller_config_file_dir, "status_light_controller"],
)

vectornav_node = Node(
package="vectornav",
executable="vectornav"
)

imu_parser_node = Node(
package="urc_platform",
executable="urc_platform_IMUParser"
)

# load_arm_controller = Node(
# package="controller_manager",
# executable="spawner",
Expand Down Expand Up @@ -160,6 +170,8 @@ def generate_launch_description():
launch_gps,
launch_imu,
rosbridge_server_node,
odom_frame_node
odom_frame_node,
vectornav_node,
imu_parser_node
]
)
9 changes: 9 additions & 0 deletions urc_platform/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(diagnostic_updater REQUIRED)
find_package(usb_cam REQUIRED)
find_package(vectornav_msgs REQUIRED)

include_directories(
include
Expand All @@ -19,6 +20,7 @@ include_directories(
# Library creation
add_library(${PROJECT_NAME} SHARED
src/joystick_driver.cpp
src/imu_parser.cpp
)

set(dependencies
Expand All @@ -28,6 +30,7 @@ set(dependencies
sensor_msgs
diagnostic_updater
usb_cam
vectornav_msgs
)

ament_target_dependencies(${PROJECT_NAME}
Expand All @@ -41,6 +44,12 @@ rclcpp_components_register_node(
EXECUTABLE ${PROJECT_NAME}_JoystickDriver
)

rclcpp_components_register_node(
${PROJECT_NAME}
PLUGIN "imu_parser::IMUParser"
EXECUTABLE ${PROJECT_NAME}_IMUParser
)

# Install launch files.
install(
DIRECTORY
Expand Down
33 changes: 33 additions & 0 deletions urc_platform/include/imu_parser.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#ifndef IMU_PARSER_H
#define IMU_PARSER_H

#include <bits/stdc++.h>
#include <math.h>
#include <rclcpp/qos.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <unistd.h>
#include <vectornav_msgs/msg/common_group.hpp>

namespace imu_parser
{

class IMUParser : public rclcpp::Node
{
public:
explicit IMUParser(const rclcpp::NodeOptions & options);

private:
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr
imu_publisher;

rclcpp::Subscription<vectornav_msgs::msg::CommonGroup>::SharedPtr vectornav_subscriber;

void VectornavCallback(const vectornav_msgs::msg::CommonGroup & msg);

};

} // namespace imu_parser

#endif
1 change: 1 addition & 0 deletions urc_platform/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<depend>usb_cam</depend>
<depend>image_transport</depend>
<depend>image_transport_plugins</depend>
<depend>vectornav_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
32 changes: 32 additions & 0 deletions urc_platform/src/imu_parser.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
#include "imu_parser.hpp"

namespace imu_parser
{

int header = 0;
IMUParser::IMUParser(const rclcpp::NodeOptions & options)
: rclcpp::Node("imu_parser", options)
{
vectornav_subscriber = create_subscription<vectornav_msgs::msg::CommonGroup>(
"/vectornav/raw/common", rclcpp::SystemDefaultsQoS(),
[this](const vectornav_msgs::msg::CommonGroup msg) {VectornavCallback(msg);});

imu_publisher = create_publisher<sensor_msgs::msg::Imu>(
"/imu", rclcpp::SystemDefaultsQoS());
}

void IMUParser::VectornavCallback(const vectornav_msgs::msg::CommonGroup & msg)
{
sensor_msgs::msg::Imu result;
// result.header.seq = header++;
result.header.frame_id = std::to_string(header);
result.orientation.x = msg.quaternion.x;
result.orientation.y = msg.quaternion.y;
result.orientation.z = msg.quaternion.z;
result.orientation.w = msg.quaternion.w;
imu_publisher->publish(result);
}

} // namespace orchestrator

RCLCPP_COMPONENTS_REGISTER_NODE(imu_parser::IMUParser)

0 comments on commit 0c2c8e9

Please sign in to comment.