restamped utlidar publisher is added
This commit is contained in:
Rooholla-KhorramBakht
parent
8ef15c19a8
commit
434004d96c
|
|
@ -4,4 +4,4 @@ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
|||
export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
|
||||
<NetworkInterface name="eth0" priority="default" multicast="default" />
|
||||
</Interfaces></General></Domain></CycloneDDS>'
|
||||
source /bridge_ws/install/setup.bash && python3 /home/front-camera-publisher.py
|
||||
source /bridge_ws/install/setup.bash && ros2 launch /root/launch/bridge.launch.py
|
||||
|
|
@ -20,8 +20,8 @@ def generate_launch_description():
|
|||
'depth_module.emitter_enabled': 1,
|
||||
'rgb_camera.color_profile':'640x480x30',
|
||||
'depth_module.depth_profile': '640x480x30',
|
||||
'enable_gyro': True,
|
||||
'enable_accel': True,
|
||||
'enable_gyro': False,
|
||||
'enable_accel': False,
|
||||
'gyro_fps': 400,
|
||||
'accel_fps': 200,
|
||||
'unite_imu_method': 2,
|
||||
|
|
|
|||
|
|
@ -1,13 +1,14 @@
|
|||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from std_msgs.msg import Header
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
|
||||
class ImagePublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__('image_publisher')
|
||||
self.publisher_ = self.create_publisher(Image, '/go2/rgb/image_raw', 10)
|
||||
self.publisher_ = self.create_publisher(Image, '/go2/front/image_raw', 10)
|
||||
self.bridge = CvBridge()
|
||||
self.gstreamer_str = "udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1"
|
||||
self.cap = cv2.VideoCapture(0)
|
||||
|
|
@ -18,6 +19,10 @@ class ImagePublisher(Node):
|
|||
if ret:
|
||||
print('new frame')
|
||||
msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
|
||||
header = Header()
|
||||
header.frame_id = 'go2_front_cam'
|
||||
header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header = header
|
||||
self.publisher_.publish(msg)
|
||||
self.get_logger().info('Publishing image')
|
||||
else:
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
|
||||
|
||||
// headers needed for highlevel control
|
||||
|
|
@ -39,7 +40,9 @@ class Custom: public rclcpp::Node
|
|||
pub_imu = this->create_publisher<sensor_msgs::msg::Imu>("/go2/imu", 1);
|
||||
pub_joint = this->create_publisher<sensor_msgs::msg::JointState>("/go2/joint_states", 1);
|
||||
nav2_twist_subr = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
|
||||
pub_foot_contact = this->create_publisher<sensor_msgs::msg::JointState>("/go2/foot_contact", 1);
|
||||
pub_foot_contact = this->create_publisher<sensor_msgs::msg::JointState>("/go2/foot_contact", 1);
|
||||
pub_utlidar = this->create_publisher<sensor_msgs::msg::PointCloud2>("/go2/utlidar", 1);
|
||||
utlidar_subr = this->create_subscription<sensor_msgs::msg::PointCloud2>("/utlidar/cloud", 1, std::bind(&Custom::utlidar_callback, this, std::placeholders::_1));
|
||||
//Unitree Topics
|
||||
init_lowcmd();
|
||||
unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
|
||||
|
|
@ -77,6 +80,8 @@ class Custom: public rclcpp::Node
|
|||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint;
|
||||
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
|
||||
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_foot_contact;
|
||||
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr utlidar_subr;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_utlidar;
|
||||
|
||||
// Unitree Interface
|
||||
unitree_api::msg::Request highreq;
|
||||
|
|
@ -96,6 +101,13 @@ class Custom: public rclcpp::Node
|
|||
rclcpp::Subscription<go2py_messages::msg::Go2pyHighCmd>::SharedPtr go2py_high_cmd_suber;
|
||||
rclcpp::Publisher<go2py_messages::msg::Go2pyState>::SharedPtr go2py_state_puber;
|
||||
rclcpp::Publisher<go2py_messages::msg::Go2pyStatus>::SharedPtr status_publisher;
|
||||
// PointCloud2
|
||||
void utlidar_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
|
||||
{
|
||||
auto restamped_msg = sensor_msgs::msg::PointCloud2(*msg);
|
||||
restamped_msg.header.stamp = this->now();
|
||||
pub_utlidar->publish(restamped_msg);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,47 @@
|
|||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Image
|
||||
from cv_bridge import CvBridge
|
||||
import cv2
|
||||
from argparse import ArgumentParser
|
||||
|
||||
argpars = ArgumentParser()
|
||||
argpars.add_argument('video_index', type=int,help="The video index of the camera", default=1)
|
||||
argpars.add_argument('topic_name', type=int,help="The topic name corresponding to the given camera.", default=1)
|
||||
args = argpars.parse_args()
|
||||
|
||||
class ImagePublisher(Node):
|
||||
def __init__(self):
|
||||
super().__init__('image_publisher')
|
||||
self.publisher_ = self.create_publisher(Image, args.topic_name, 10)
|
||||
self.bridge = CvBridge()
|
||||
self.cap = cv2.VideoCapture(args.video_index)
|
||||
|
||||
def publish_images(self):
|
||||
while rclpy.ok():
|
||||
ret, frame = self.cap.read()
|
||||
if ret:
|
||||
print('new frame')
|
||||
msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
|
||||
self.publisher_.publish(msg)
|
||||
self.get_logger().info('Publishing image')
|
||||
else:
|
||||
self.get_logger().warn('Failed to capture image')
|
||||
break
|
||||
|
||||
self.cap.release()
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
image_publisher = ImagePublisher()
|
||||
|
||||
try:
|
||||
image_publisher.publish_images()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
image_publisher.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -1,3 +1,4 @@
|
|||
/usr/sbin/modprobe v4l2loopback
|
||||
# Run the GStreamer pipeline
|
||||
/usr/bin/gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! queue ! application/x-rtp, media=video, \
|
||||
encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! v4l2sink device=/dev/video0 &
|
||||
|
|
|
|||
|
|
@ -7,5 +7,5 @@ RUN --mount=type=cache,target=/var/cache/apt \
|
|||
|
||||
RUN apt install -y ros-humble-isaac-ros-h264-decoder ros-humble-isaac-ros-h264-encoder
|
||||
|
||||
RUN mkdir -p /realsense-ws/src && cd /realsense-ws/src && git clone https://github.com/IntelRealSense/realsense-ros.git && cd .. && source /opt/ros/humble/setup.bash && colcon build
|
||||
#RUN mkdir -p /realsense-ws/src && cd /realsense-ws/src && git clone https://github.com/IntelRealSense/realsense-ros.git && cd .. && source /opt/ros/humble/setup.bash && colcon build
|
||||
USER $USERNAME
|
||||
|
|
|
|||
Loading…
Reference in New Issue