Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement PTZ action server interface #65

Merged
merged 3 commits into from
Dec 10, 2024
Merged

Implement PTZ action server interface #65

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ec2f9f6
Add remappings & bridge configuration for the PTZ joint states
civerachb-cpr Dec 6, 2024
b09e30a
Remap the raw velocity topics from Gazebo, implement the PTZ action s…
civerachb-cpr Dec 6, 2024
b4d3d05
Fix duplicate dependency
civerachb-cpr Dec 10, 2024
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 clearpath_generator_gz/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ find_package(ament_cmake_python REQUIRED)

install(PROGRAMS
${PROJECT_NAME}/launch/generate_launch
${PROJECT_NAME}/nodes/ptz_controller_node
${PROJECT_NAME}/param/generate_param
DESTINATION lib/${PROJECT_NAME}
)
Expand Down
59 changes: 56 additions & 3 deletions clearpath_generator_gz/clearpath_generator_gz/launch/sensors.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ class SensorLaunch():
GZ_TO_ROS_CAMERA_INFO = '@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo'
GZ_TO_ROS_IMU = '@sensor_msgs/msg/Imu[gz.msgs.IMU'
GZ_TO_ROS_NAVSAT = '@sensor_msgs/msg/NavSatFix[gz.msgs.NavSat'
GZ_TO_ROS_JOINTSTATE = '@sensor_msgs/msg/JointState[gz.msgs.Model'

ROS_TO_GZ_FLOAT = '@std_msgs/msg/Float64]gz.msgs.Double'

Expand Down Expand Up @@ -105,7 +106,10 @@ def __init__(
self.extra_gz_nodes = []

# Cameras
if self.sensor.SENSOR_TYPE == BaseCamera.SENSOR_TYPE:
if (
self.sensor.SENSOR_TYPE == BaseCamera.SENSOR_TYPE and
self.sensor.SENSOR_MODEL not in self.PTZ_CAMERAS
):
image_ns = '/' + self.namespace + self.name
image_topic = image_ns + '/image'

Expand Down Expand Up @@ -150,20 +154,69 @@ def __init__(
self.extra_gz_nodes.append(depth_bridge_node)

if self.sensor.SENSOR_MODEL in self.PTZ_CAMERAS:
image_ns = '/' + self.namespace + self.name
image_topic = image_ns + '/image'

image_bridge_node = LaunchFile.Node(
name=self.name + '_gz_image_bridge',
namespace=self.namespace,
package='ros_gz_image',
executable='image_bridge',
parameters=[{
'use_sim_time': True,
}],
arguments=[image_topic],
remappings=[
(image_topic, image_ns + '/_/image_raw'),
(image_topic + '/compressed', image_ns + '/_/compressed'),
(image_topic + '/compressedDepth', image_ns + '/_/compressedDepth'),
(image_topic + '/theora', image_ns + '/_/theora'),
]
)
self.extra_gz_nodes.append(image_bridge_node)

cmd_ns = '/' + self.namespace + self.name
cmd_bridge_node = LaunchFile.Node(
name=self.name + '_gz_cmd_node',
name=self.name + '_gz_cmd_bridge',
namespace=self.namespace,
package='ros_gz_bridge',
executable='parameter_bridge',
parameters=[{'use_sim_time': True}],
arguments = [
cmd_ns + '/cmd_pan_vel' + self.ROS_TO_GZ_FLOAT,
cmd_ns + '/cmd_tilt_vel' + self.ROS_TO_GZ_FLOAT,
]
cmd_ns + '/pan_joint_state' + self.GZ_TO_ROS_JOINTSTATE,
cmd_ns + '/tilt_joint_state' + self.GZ_TO_ROS_JOINTSTATE,
],
remappings=[
(cmd_ns + '/pan_joint_state', '/' + self._robot_namespace + '/platform/joint_states'),
(cmd_ns + '/tilt_joint_state', '/' + self._robot_namespace + '/platform/joint_states'),
(cmd_ns + '/cmd_pan_vel', cmd_ns + '/_/cmd_pan_vel'),
(cmd_ns + '/cmd_tilt_vel', cmd_ns + '/_/cmd_tilt_vel'),
],
)
self.extra_gz_nodes.append(cmd_bridge_node)

ptz_node = LaunchFile.Node(
name='ptz_action_server_node',
namespace=image_ns,
package='clearpath_generator_gz',
executable='ptz_controller_node',
parameters=[
{'use_sim_time': True},
{'camera_name': self.name},
],
remappings=[
('image_in', image_ns + '/_/image_raw'),
('image_out', image_ns + '/color/image'),
('cmd/velocity', image_ns + '/cmd/velocity'),
('joint_states', '/' + self._robot_namespace + '/platform/joint_states'),
('cmd_pan_vel', cmd_ns + '/_/cmd_pan_vel'),
('cmd_tilt_vel', cmd_ns + '/_/cmd_tilt_vel'),
],
)
self.extra_gz_nodes.append(ptz_node)

def generate(self):
sensor_writer = LaunchWriter(self.launch_file)
# Add sensor bridge and tf nodes
Expand Down
Loading
Loading