Add lider ROS node and move robot control into its own directory
This commit is contained in:
@@ -0,0 +1,59 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# ── Motor controller arguments ────────────────────────────────────
|
||||
DeclareLaunchArgument('wheel_base', default_value='0.3',
|
||||
description='Distance between left and right wheels (m)'),
|
||||
DeclareLaunchArgument('max_speed', default_value='1.0',
|
||||
description='Maximum motor speed in library units'),
|
||||
|
||||
# ── Ultrasonic sensor arguments ───────────────────────────────────
|
||||
DeclareLaunchArgument('ultrasonic_rate_hz', default_value='10.0',
|
||||
description='Ultrasonic sensor publish rate (Hz)'),
|
||||
|
||||
# ── Camera orientation arguments ──────────────────────────────────
|
||||
DeclareLaunchArgument('pan_center_deg', default_value='90.0',
|
||||
description='Pan angle at startup and shutdown (degrees)'),
|
||||
DeclareLaunchArgument('tilt_center_deg', default_value='60.0',
|
||||
description='Tilt angle at startup and shutdown (degrees)'),
|
||||
|
||||
# ── Nodes ─────────────────────────────────────────────────────────
|
||||
Node(
|
||||
package='raspbot_v2',
|
||||
executable='motor_controller',
|
||||
name='motor_controller',
|
||||
parameters=[{
|
||||
'wheel_base': LaunchConfiguration('wheel_base'),
|
||||
'max_speed': LaunchConfiguration('max_speed'),
|
||||
}],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
Node(
|
||||
package='raspbot_v2',
|
||||
executable='camera_orientation',
|
||||
name='camera_orientation',
|
||||
parameters=[{
|
||||
'pan_center_deg': LaunchConfiguration('pan_center_deg'),
|
||||
'tilt_center_deg': LaunchConfiguration('tilt_center_deg'),
|
||||
}],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
Node(
|
||||
package='raspbot_v2',
|
||||
executable='ultrasonic',
|
||||
name='ultrasonic',
|
||||
parameters=[{
|
||||
'publish_rate_hz': LaunchConfiguration('ultrasonic_rate_hz'),
|
||||
}],
|
||||
output='screen',
|
||||
),
|
||||
|
||||
])
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="3">
|
||||
<name>raspbot_v2</name>
|
||||
<version>0.0.1</version>
|
||||
<description>Yahboom Raspbot V2 motor and camera orientation controller</description>
|
||||
<maintainer email="you@example.com">Your Name</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
</package>
|
||||
@@ -0,0 +1,199 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Camera orientation controller node.
|
||||
|
||||
Subscribes to joint commands and drives the pan/tilt servo pair on the
|
||||
Raspbot via Ctrl_Servo. Publishes current joint state for downstream
|
||||
consumers (e.g. tf2, rviz2).
|
||||
|
||||
Standard interfaces
|
||||
-------------------
|
||||
Command : /joint_command (sensor_msgs/JointState)
|
||||
Joint names: "pan" – horizontal rotation (yaw)
|
||||
Joint names: "tilt" – vertical rotation (pitch)
|
||||
Positions in **radians** (ROS convention).
|
||||
|
||||
State : /joint_states (sensor_msgs/JointState)
|
||||
Same joint names; position in radians, reflecting the last
|
||||
commanded angle.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
pan_servo_id int default 1 – Raspbot servo channel for pan
|
||||
tilt_servo_id int default 2 – Raspbot servo channel for tilt
|
||||
pan_min_deg float default 0.0 – hardware lower limit for pan (°)
|
||||
pan_max_deg float default 180.0
|
||||
tilt_min_deg float default 0.0 – hardware lower limit for tilt (°)
|
||||
tilt_max_deg float default 110.0 – library caps tilt at 110°
|
||||
pan_center_deg float default 90.0 – position commanded on startup
|
||||
tilt_center_deg float default 60.0
|
||||
state_rate_hz float default 10.0 – joint-state publish rate
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
|
||||
# Servo IDs used by Ctrl_Servo
|
||||
_PAN_SERVO_ID = 1
|
||||
_TILT_SERVO_ID = 2
|
||||
|
||||
|
||||
def _deg_to_rad(deg: float) -> float:
|
||||
return deg * math.pi / 180.0
|
||||
|
||||
|
||||
def _rad_to_deg(rad: float) -> float:
|
||||
return rad * 180.0 / math.pi
|
||||
|
||||
|
||||
class CameraOrientationNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('camera_orientation')
|
||||
|
||||
# --- Parameters ---
|
||||
self.declare_parameter('pan_servo_id', _PAN_SERVO_ID)
|
||||
self.declare_parameter('tilt_servo_id', _TILT_SERVO_ID)
|
||||
self.declare_parameter('pan_min_deg', 0.0)
|
||||
self.declare_parameter('pan_max_deg', 180.0)
|
||||
self.declare_parameter('tilt_min_deg', 0.0)
|
||||
self.declare_parameter('tilt_max_deg', 110.0)
|
||||
self.declare_parameter('pan_center_deg', 90.0)
|
||||
self.declare_parameter('tilt_center_deg', 60.0)
|
||||
self.declare_parameter('state_rate_hz', 10.0)
|
||||
|
||||
self._pan_id = self.get_parameter('pan_servo_id').value
|
||||
self._tilt_id = self.get_parameter('tilt_servo_id').value
|
||||
self._pan_min = self.get_parameter('pan_min_deg').value
|
||||
self._pan_max = self.get_parameter('pan_max_deg').value
|
||||
self._tilt_min = self.get_parameter('tilt_min_deg').value
|
||||
self._tilt_max = self.get_parameter('tilt_max_deg').value
|
||||
|
||||
pan_center = self.get_parameter('pan_center_deg').value
|
||||
tilt_center = self.get_parameter('tilt_center_deg').value
|
||||
rate_hz = self.get_parameter('state_rate_hz').value
|
||||
|
||||
# --- Hardware ---
|
||||
# Import here so the node can be unit-tested without hardware present
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot
|
||||
self._bot = Raspbot()
|
||||
|
||||
# --- State (degrees, internal representation) ---
|
||||
self._pan_deg = pan_center
|
||||
self._tilt_deg = tilt_center
|
||||
|
||||
# Drive servos to the startup centre position
|
||||
self._apply(self._pan_deg, self._tilt_deg)
|
||||
|
||||
# --- Subscriber ---
|
||||
self._cmd_sub = self.create_subscription(
|
||||
JointState,
|
||||
'joint_command',
|
||||
self._joint_command_cb,
|
||||
10,
|
||||
)
|
||||
|
||||
# --- Publisher ---
|
||||
self._state_pub = self.create_publisher(JointState, 'joint_states', 10)
|
||||
self._state_timer = self.create_timer(1.0 / rate_hz, self._publish_state)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Camera orientation node started '
|
||||
f'(pan servo {self._pan_id}, tilt servo {self._tilt_id}). '
|
||||
f'Centred at pan={pan_center}° tilt={tilt_center}°.'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Subscriber callback
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _joint_command_cb(self, msg: JointState):
|
||||
"""
|
||||
Accept a JointState command. Only joints named "pan" or "tilt"
|
||||
are acted on; other names are silently ignored so this node can
|
||||
coexist on a shared joint_command topic.
|
||||
|
||||
Positions are expected in radians (ROS convention).
|
||||
"""
|
||||
if len(msg.name) != len(msg.position):
|
||||
self.get_logger().warn(
|
||||
'joint_command message has mismatched name/position lengths; ignoring.'
|
||||
)
|
||||
return
|
||||
|
||||
pan_deg = self._pan_deg
|
||||
tilt_deg = self._tilt_deg
|
||||
|
||||
for name, pos_rad in zip(msg.name, msg.position):
|
||||
pos_deg = _rad_to_deg(pos_rad)
|
||||
if name == 'pan':
|
||||
pan_deg = self._clamp(pos_deg, self._pan_min, self._pan_max)
|
||||
elif name == 'tilt':
|
||||
tilt_deg = self._clamp(pos_deg, self._tilt_min, self._tilt_max)
|
||||
|
||||
self._apply(pan_deg, tilt_deg)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _apply(self, pan_deg: float, tilt_deg: float):
|
||||
"""Send angles to hardware and update internal state."""
|
||||
pan_int = int(round(pan_deg))
|
||||
tilt_int = int(round(tilt_deg))
|
||||
try:
|
||||
self._bot.Ctrl_Servo(self._pan_id, pan_int)
|
||||
self._bot.Ctrl_Servo(self._tilt_id, tilt_int)
|
||||
self._pan_deg = pan_deg
|
||||
self._tilt_deg = tilt_deg
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f'Servo write failed: {exc}')
|
||||
|
||||
@staticmethod
|
||||
def _clamp(value: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, value))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# State publisher
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _publish_state(self):
|
||||
msg = JointState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.name = ['pan', 'tilt']
|
||||
msg.position = [_deg_to_rad(self._pan_deg), _deg_to_rad(self._tilt_deg)]
|
||||
self._state_pub.publish(msg)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Shutdown
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def destroy_node(self):
|
||||
self.get_logger().info('Parking camera servos at centre...')
|
||||
pan_center = self.get_parameter('pan_center_deg').value
|
||||
tilt_center = self.get_parameter('tilt_center_deg').value
|
||||
try:
|
||||
self._apply(pan_center, tilt_center)
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = CameraOrientationNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,119 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
|
||||
# Import your motor library
|
||||
from raspbot_v2_interface import MotorController
|
||||
|
||||
class MotorControllerNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('motor_controller')
|
||||
|
||||
# --- Parameters ---
|
||||
self.declare_parameter('wheel_base', 0.3) # meters between left/right wheels
|
||||
self.declare_parameter('max_speed', 1.0) # max motor speed (library units)
|
||||
|
||||
self.wheel_base = self.get_parameter('wheel_base').value
|
||||
self.max_speed = self.get_parameter('max_speed').value
|
||||
|
||||
# --- Initialize your motor library ---
|
||||
self.motors = MotorController()
|
||||
self.motors.initialize()
|
||||
|
||||
# --- Subscribers ---
|
||||
# cmd_vel is the standard ROS2 topic for velocity commands
|
||||
self.cmd_vel_sub = self.create_subscription(
|
||||
Twist,
|
||||
'cmd_vel',
|
||||
self.cmd_vel_callback,
|
||||
10 # QoS queue depth
|
||||
)
|
||||
|
||||
# Optional: direct per-wheel speed control [FL, FR, RL, RR]
|
||||
self.wheel_speeds_sub = self.create_subscription(
|
||||
Float32MultiArray,
|
||||
'wheel_speeds',
|
||||
self.wheel_speeds_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# --- Publishers ---
|
||||
# Publish current wheel speeds for feedback
|
||||
self.speed_pub = self.create_publisher(Float32MultiArray, 'current_wheel_speeds', 10)
|
||||
|
||||
# --- Timer for telemetry ---
|
||||
self.telemetry_timer = self.create_timer(0.1, self.publish_telemetry) # 10 Hz
|
||||
|
||||
self.get_logger().info('Motor controller node started')
|
||||
|
||||
def cmd_vel_callback(self, msg: Twist):
|
||||
"""
|
||||
Convert a Twist message (linear.x, angular.z) into
|
||||
differential-drive wheel speeds for a 4-wheeled robot.
|
||||
"""
|
||||
linear = msg.linear.x # m/s forward
|
||||
angular = msg.angular.z # rad/s rotation
|
||||
|
||||
# Differential drive kinematics
|
||||
left_speed = linear - (angular * self.wheel_base / 2.0)
|
||||
right_speed = linear + (angular * self.wheel_base / 2.0)
|
||||
|
||||
# Clamp to max speed
|
||||
left_speed = max(-self.max_speed, min(self.max_speed, left_speed))
|
||||
right_speed = max(-self.max_speed, min(self.max_speed, right_speed))
|
||||
|
||||
# Send to motors via your library (FL=FR=left, RL=RR=right)
|
||||
try:
|
||||
self.motors.set_speed(front_left=left_speed, front_right=right_speed,
|
||||
rear_left=left_speed, rear_right=right_speed)
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Motor error: {e}')
|
||||
|
||||
def wheel_speeds_callback(self, msg: Float32MultiArray):
|
||||
"""Direct per-wheel control: [FL, FR, RL, RR]"""
|
||||
if len(msg.data) != 4:
|
||||
self.get_logger().warn('wheel_speeds expects exactly 4 values [FL, FR, RL, RR]')
|
||||
return
|
||||
try:
|
||||
self.motors.set_speed(front_left=msg.data[0], front_right=msg.data[1],
|
||||
rear_left=msg.data[2], rear_right=msg.data[3])
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Motor error: {e}')
|
||||
|
||||
def publish_telemetry(self):
|
||||
"""Read and publish current wheel speeds from your library."""
|
||||
try:
|
||||
speeds = self.motors.get_speeds() # Expected: [FL, FR, RL, RR]
|
||||
msg = Float32MultiArray()
|
||||
msg.data = [float(s) for s in speeds]
|
||||
self.speed_pub.publish(msg)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Telemetry read failed: {e}')
|
||||
|
||||
def destroy_node(self):
|
||||
"""Clean shutdown — stop motors before exiting."""
|
||||
self.get_logger().info('Shutting down motors...')
|
||||
try:
|
||||
self.motors.stop_all()
|
||||
self.motors.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = MotorControllerNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,167 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Ultrasonic range sensor node.
|
||||
|
||||
Publishes distance readings from the Raspbot's HC-SR04-style sensor as a
|
||||
standard sensor_msgs/Range message. The sensor is only powered on while at
|
||||
least one subscriber is active, and automatically powered off when the last
|
||||
subscriber disconnects.
|
||||
|
||||
Standard interface
|
||||
------------------
|
||||
Published : /ultrasonic/range (sensor_msgs/Range)
|
||||
radiation_type = ULTRASOUND
|
||||
range in metres; +inf when target is beyond max_range,
|
||||
-inf when closer than min_range (REP-117 convention).
|
||||
|
||||
Parameters
|
||||
----------
|
||||
publish_rate_hz float default 10.0 – sensor poll and publish rate
|
||||
frame_id str default 'ultrasonic' – header frame_id
|
||||
min_range_m float default 0.02 – minimum valid range (m)
|
||||
max_range_m float default 4.0 – maximum valid range (m)
|
||||
field_of_view float default 0.2618 – sensor cone width (rad, ~15°)
|
||||
warmup_s float default 1.0 – seconds to wait after powering the
|
||||
sensor on before trusting readings
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import Range
|
||||
|
||||
|
||||
class UltrasonicNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('ultrasonic')
|
||||
|
||||
# --- Parameters ---
|
||||
self.declare_parameter('publish_rate_hz', 10.0)
|
||||
self.declare_parameter('frame_id', 'ultrasonic')
|
||||
self.declare_parameter('min_range_m', 0.02)
|
||||
self.declare_parameter('max_range_m', 4.0)
|
||||
self.declare_parameter('field_of_view', 0.2618) # ~15 degrees
|
||||
self.declare_parameter('warmup_s', 1.0)
|
||||
|
||||
self._frame_id = self.get_parameter('frame_id').value
|
||||
self._min_range = self.get_parameter('min_range_m').value
|
||||
self._max_range = self.get_parameter('max_range_m').value
|
||||
self._fov = self.get_parameter('field_of_view').value
|
||||
self._warmup_s = self.get_parameter('warmup_s').value
|
||||
rate_hz = self.get_parameter('publish_rate_hz').value
|
||||
|
||||
# --- Hardware ---
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot
|
||||
self._bot = Raspbot()
|
||||
|
||||
# --- Sensor state ---
|
||||
self._sensor_on = False
|
||||
self._enabled_at = 0.0 # monotonic time when sensor was switched on
|
||||
|
||||
# --- Publisher ---
|
||||
self._pub = self.create_publisher(Range, 'ultrasonic/range', 10)
|
||||
|
||||
# --- Timer ---
|
||||
self._timer = self.create_timer(1.0 / rate_hz, self._tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Ultrasonic node started ({rate_hz} Hz, '
|
||||
f'range {self._min_range}–{self._max_range} m). '
|
||||
f'Sensor will activate when subscribers connect.'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Timer callback
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _tick(self):
|
||||
has_subscribers = self._pub.get_subscription_count() > 0
|
||||
|
||||
# Power the sensor on/off based on subscriber presence
|
||||
if has_subscribers and not self._sensor_on:
|
||||
self._set_sensor(True)
|
||||
return # skip this tick — let the sensor warm up
|
||||
|
||||
if not has_subscribers and self._sensor_on:
|
||||
self._set_sensor(False)
|
||||
return
|
||||
|
||||
if not self._sensor_on:
|
||||
return
|
||||
|
||||
# Still warming up — do not publish unreliable readings
|
||||
if time.monotonic() - self._enabled_at < self._warmup_s:
|
||||
return
|
||||
|
||||
# Read and publish
|
||||
raw_mm = self._bot.Read_Ultrasonic()
|
||||
if raw_mm is None:
|
||||
self.get_logger().warn('Ultrasonic read returned None', throttle_duration_sec=5.0)
|
||||
return
|
||||
|
||||
self._pub.publish(self._build_msg(raw_mm))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _set_sensor(self, on: bool):
|
||||
try:
|
||||
self._bot.Ctrl_Ulatist_Switch(1 if on else 0)
|
||||
self._sensor_on = on
|
||||
self._enabled_at = time.monotonic()
|
||||
self.get_logger().info(f'Ultrasonic sensor {"enabled" if on else "disabled"}')
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f'Failed to set sensor state: {exc}')
|
||||
|
||||
def _build_msg(self, raw_mm: int) -> Range:
|
||||
msg = Range()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self._frame_id
|
||||
msg.radiation_type = Range.ULTRASOUND
|
||||
msg.field_of_view = self._fov
|
||||
msg.min_range = self._min_range
|
||||
msg.max_range = self._max_range
|
||||
|
||||
distance_m = raw_mm / 1000.0
|
||||
|
||||
# REP-117: report +inf / -inf for out-of-range readings
|
||||
if distance_m > self._max_range:
|
||||
msg.range = math.inf
|
||||
elif distance_m < self._min_range:
|
||||
msg.range = -math.inf
|
||||
else:
|
||||
msg.range = distance_m
|
||||
|
||||
return msg
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Shutdown
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def destroy_node(self):
|
||||
if self._sensor_on:
|
||||
try:
|
||||
self._bot.Ctrl_Ulatist_Switch(0)
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = UltrasonicNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,13 @@
|
||||
[metadata]
|
||||
name = raspbot_v2
|
||||
version = 0.0.1
|
||||
|
||||
[options]
|
||||
packages = find:
|
||||
install_requires =
|
||||
setuptools
|
||||
|
||||
[develop]
|
||||
script_dir=$base/lib/raspbot_v2
|
||||
[install]
|
||||
install_scripts=$base/lib/raspbot_v2
|
||||
@@ -0,0 +1,23 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = 'raspbot_v2'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.0.1',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
('share/' + package_name + '/launch', ['launch/robot.launch.py']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'motor_controller = raspbot_v2.motor_controller_node:main',
|
||||
'camera_orientation = raspbot_v2.camera_orientation_node:main',
|
||||
'ultrasonic = raspbot_v2.ultrasonic_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user