Skip to content

Commit

Permalink
Renamed msgs
Browse files Browse the repository at this point in the history
Signed-off-by: Juancams <jc.manzanares.serrano@gmail.com>
  • Loading branch information
Juancams committed Jun 27, 2024
1 parent a3bf6e1 commit 8371fa8
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 11 deletions.
14 changes: 7 additions & 7 deletions go2_driver/include/go2_driver/go2_driver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include "go2_interfaces/msg/go2_state.hpp"
#include "go2_interfaces/msg/low_state.hpp"
#include "go2_interfaces/msg/imu_state.hpp"
#include "unitree_go/msg/go2_state.hpp"
#include "unitree_go/msg/low_state.hpp"
#include "unitree_go/msg/imu_state.hpp"
#include "tf2_ros/transform_broadcaster.h"

namespace go2_driver
Expand All @@ -56,21 +56,21 @@ class Go2Driver : public rclcpp::Node
void publish_lidar_cyclonedds(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
void publish_body_poss_cyclonedds(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg);
void publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg);
void publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg);
void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);


rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr robot_pose_sub_;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
rclcpp::Subscription<go2_interfaces::msg::LowState>::SharedPtr low_state_sub_;
rclcpp::Subscription<unitree_go::msg::LowState>::SharedPtr low_state_sub_;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_state_pub_;
rclcpp::Publisher<go2_interfaces::msg::Go2State>::SharedPtr go2_state_pub_;
rclcpp::Publisher<unitree_go::msg::Go2State>::SharedPtr go2_state_pub_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
rclcpp::Publisher<go2_interfaces::msg::IMUState>::SharedPtr imu_pub_;
rclcpp::Publisher<unitree_go::msg::IMUState>::SharedPtr imu_pub_;

std::string robot_ip_;
std::string token_;
Expand Down
8 changes: 4 additions & 4 deletions go2_driver/src/go2_driver/go2_driver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ Go2Driver::Go2Driver(

pointcloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("pointcloud", 10);
joint_state_pub_ = create_publisher<sensor_msgs::msg::JointState>("joint_states", 10);
go2_state_pub_ = create_publisher<go2_interfaces::msg::Go2State>("go2_states", 10);
go2_state_pub_ = create_publisher<unitree_go::msg::Go2State>("go2_states", 10);
odom_pub_ = create_publisher<nav_msgs::msg::Odometry>("odom", 10);
imu_pub_ = create_publisher<go2_interfaces::msg::IMUState>("imu", 10);
imu_pub_ = create_publisher<unitree_go::msg::IMUState>("imu", 10);

pointcloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"/utlidar/cloud", 10,
Expand All @@ -66,7 +66,7 @@ Go2Driver::Go2Driver(
joy_sub_ = create_subscription<sensor_msgs::msg::Joy>(
"joy", 10, std::bind(&Go2Driver::joy_callback, this, std::placeholders::_1));

low_state_sub_ = create_subscription<go2_interfaces::msg::LowState>(
low_state_sub_ = create_subscription<unitree_go::msg::LowState>(
"lowstate", 10,
std::bind(&Go2Driver::publish_joint_state_cyclonedds, this, std::placeholders::_1));

Expand Down Expand Up @@ -102,7 +102,7 @@ void Go2Driver::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg)
joy_state_ = *msg;
}

void Go2Driver::publish_joint_state_cyclonedds(const go2_interfaces::msg::LowState::SharedPtr msg)
void Go2Driver::publish_joint_state_cyclonedds(const unitree_go::msg::LowState::SharedPtr msg)
{
RCLCPP_INFO(get_logger(), "Received lowstate message");
sensor_msgs::msg::JointState joint_state;
Expand Down

0 comments on commit 8371fa8

Please sign in to comment.