From b5d0c5b80b713344277d4c9b271a308386079185 Mon Sep 17 00:00:00 2001 From: Arend-Jan van Hilten Date: Wed, 29 May 2024 13:34:40 +0200 Subject: [PATCH] Revert "Revert "Mirte master namespace-able"" --- README.md | 48 +++---- .../include/my_robot_hw_interface.h | 24 ++-- .../rrbot_control/src/rrbot_hw_interface.cpp | 24 ++-- .../scripts/ROS_telemetrix_aio_api.py | 120 +++++++++--------- mirte_telemetrix/scripts/modules/MPU9250.py | 4 +- mirte_teleop/launch/teleop_holo_joy.launch | 2 +- mirte_teleop/launch/teleop_joy.launch | 2 +- mirte_teleop/launch/teleopkey.launch | 2 +- mirte_teleop/scripts/joyArm.py | 18 +-- 9 files changed, 122 insertions(+), 122 deletions(-) diff --git a/README.md b/README.md index 3699884b..3847a184 100644 --- a/README.md +++ b/README.md @@ -1,26 +1,26 @@ # API -The motors are controlled with PWM as as service. By default the topic published by the ROS diff_drive_controller (/mobile_base_controller/cmd_vel) will call that service. One has to disable the ROS diff_drive_controller () to manually set the PWM signal by using the service (eg from commandline). +The motors are controlled with PWM as as service. By default the topic published by the ROS diff_drive_controller (`mobile_base_controller/cmd_vel`) will call that service. One has to disable the ROS diff_drive_controller () to manually set the PWM signal by using the service (eg from commandline). Currently, the default parameters are set to use the Mirte Arduino-nano PCB. ## Subscribed Topics -/mobile_base_controller/cmd_vel (geometry_msgs/Twist) +`mobile_base_controller/cmd_vel` (geometry_msgs/Twist) ## Published Topics Topics published to depend on the parameters given. -/mirte/_distance (sensor_msgs/Range) -/mirte/_encoder (mirte_msgs/Encoder) -/mirte/_intensity (mirte_msgs/Intensity) +`mirte/_distance` (sensor_msgs/Range) +`mirte/_encoder` (mirte_msgs/Encoder) +`mirte/_intensity` (mirte_msgs/Intensity) ## Services -/mirte_navigation/move -/mirte_navigation/turn -/mirte_pymata/set__motor_pwm -/mirte/get_pin_value -/mirte/set_pin_mode -/mirte/set_pin_value -/mirte_service_api/get_ +`mirte_navigation/move` +`mirte_navigation/turn` +`mirte_pymata/set__motor_pwm` +`mirte/get_pin_value` +`mirte/set_pin_mode` +`mirte/set_pin_value` +`mirte_service_api/get_` ## Parameters @@ -33,26 +33,26 @@ device/mirte/dev (string, default: "/dev/ttyUSB0") The linux device name of the board. #### Distance sensor -distance/_distance/dev (string, default: "mirte") -distance/_distance/frequency (int, default: 10) +`distance/_distance/dev` (string, default: "mirte") +`distance/_distance/frequency` (int, default: 10) The frequency to publish new sensor data -distance/_distance/pin (array) +`distance/_distance/pin` (array) The (arduino/stm32) pins it is connected to [trigger-pin, echo-pin] (eg [8. 13]) #### Encoder sensor Note: the encoder sensor does not have a frequency since it uses the interrupt pins -encoder/_encoder/device (string, default: "mirte") -encoder/_encoder/pin (int) -encoder/_encoder/ticks_per_wheel (int, default: 20) +`encoder/_encoder/device` (string, default: "mirte") +`encoder/_encoder/pin` (int) +`encoder/_encoder/ticks_per_wheel` (int, default: 20) #### Intensity sensor -intensity/_intensity/device (string, default: "mirte") -intensity/_intensity/frequency (int, default: 10) -intensity/_intensity/pin (int) +`intensity/_intensity/device` (string, default: "mirte") +`intensity/_intensity/frequency` (int, default: 10) +`intensity/_intensity/pin` (int) #### Motor settings -motor/_motor/device (string, default: "mirte") -motor/_motor/pin (array) +`motor/_motor/device` (string, default: "mirte") +`motor/_motor/pin` (array) The (arduino/stm32) pins the motor (h-bridge) is connected to. When using 2 pins per motor it will assume MX1919 (both PWM signals). When using 3 pins it will assume L298N (1 PWM, 2 non-PWM) @@ -72,4 +72,4 @@ clang-format --Werror ./**/**.cpp -style=llvm -i ``` # Required packages Telemetrix -Requires https://github.com/mirte-robot/tmx-pico-aio.git to be installed ( ```pip install git+https://github.com/mirte-robot/tmx-pico-aio.git``` ) for the Pico and https://github.com/mirte-robot/telemetrix-aio.git for the STM32 and Arduino Nano (```pip install git+https://github.com/mirte-robot/telemetrix-aio.git```). \ No newline at end of file +Requires https://github.com/mirte-robot/tmx-pico-aio.git to be installed ( ```pip install git+https://github.com/mirte-robot/tmx-pico-aio.git``` ) for the Pico and https://github.com/mirte-robot/telemetrix-aio.git for the STM32 and Arduino Nano (```pip install git+https://github.com/mirte-robot/telemetrix-aio.git```). diff --git a/mirte_control/mirte_base_control/include/my_robot_hw_interface.h b/mirte_control/mirte_base_control/include/my_robot_hw_interface.h index d3b7c742..a2a34b1e 100644 --- a/mirte_control/mirte_base_control/include/my_robot_hw_interface.h +++ b/mirte_control/mirte_base_control/include/my_robot_hw_interface.h @@ -37,8 +37,8 @@ #include #include // const unsigned int NUM_JOINTS = 4; -const auto service_format = "/mirte/set_%s_speed"; -const auto encoder_format = "/mirte/encoder/%s"; +const auto service_format = "mirte/set_%s_speed"; +const auto encoder_format = "mirte/encoder/%s"; const auto max_speed = 100; // Quick fix hopefully for power dip. bool equal_gains(control_toolbox::Pid::Gains lhs, @@ -239,7 +239,7 @@ void MyRobotHWInterface::init_service_clients() { unsigned int detect_joints(ros::NodeHandle &nh) { std::string type; - nh.param("/mobile_base_controller/type", type, ""); + nh.param("mobile_base_controller/type", type, ""); if (type.rfind("mecanum", 0) == 0) { // starts with mecanum return 4; } else if (type.rfind("diff", 0) == 0) { // starts with diff @@ -256,13 +256,13 @@ MyRobotHWInterface::MyRobotHWInterface() "start", &MyRobotHWInterface::start_callback, this)), stop_srv_(nh.advertiseService("stop", &MyRobotHWInterface::stop_callback, this)) { - private_nh.param("/mobile_base_controller/wheel_radius", - _wheel_diameter, 0.06); + nh.param("mobile_base_controller/wheel_radius", _wheel_diameter, + 0.06); _wheel_diameter *= 2; // convert from radius to diameter - private_nh.param("/mobile_base_controller/max_speed", _max_speed, - 2.0); // TODO: unused - private_nh.param("/mobile_base_controller/ticks", ticks, 40.0); - this->NUM_JOINTS = detect_joints(private_nh); + nh.param("mobile_base_controller/max_speed", _max_speed, + 2.0); // TODO: unused + nh.param("mobile_base_controller/ticks", ticks, 40.0); + this->NUM_JOINTS = detect_joints(nh); if (this->NUM_JOINTS > 2) { this->bidirectional = true; } @@ -314,13 +314,11 @@ MyRobotHWInterface::MyRobotHWInterface() registerInterface(&jnt_state_interface); registerInterface(&jnt_vel_interface); - private_nh.param("/mobile_base_controller/enable_pid", enablePID, - false); - enablePID = true; + nh.param("mobile_base_controller/enable_pid", enablePID, false); if (enablePID) { // dummy pid for dynamic reconfigure. this->reconfig_pid = std::make_shared(1, 0, 0); - this->reconfig_pid->initParam("/mobile_base_controller/", false); + this->reconfig_pid->initParam("mobile_base_controller/", false); auto gains = this->reconfig_pid->getGains(); for (auto i = 0; i < NUM_JOINTS; i++) { auto pid = std::make_shared(1, 1, 1); diff --git a/mirte_control/ros_control_boilerplate/rrbot_control/src/rrbot_hw_interface.cpp b/mirte_control/ros_control_boilerplate/rrbot_control/src/rrbot_hw_interface.cpp index de13e5f8..eb0d526e 100644 --- a/mirte_control/ros_control_boilerplate/rrbot_control/src/rrbot_hw_interface.cpp +++ b/mirte_control/ros_control_boilerplate/rrbot_control/src/rrbot_hw_interface.cpp @@ -90,13 +90,13 @@ RRBotHWInterface::RRBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model) : ros_control_boilerplate::GenericHWInterface(nh, urdf_model) { this->connectServices(); - sub0 = nh.subscribe("/mirte/servos/servoRot/position", 1, + sub0 = nh.subscribe("mirte/servos/servoRot/position", 1, rrbot_control::callbackJoint0); - sub1 = nh.subscribe("/mirte/servos/servoShoulder/position", 1, + sub1 = nh.subscribe("mirte/servos/servoShoulder/position", 1, rrbot_control::callbackJoint1); - sub2 = nh.subscribe("/mirte/servos/servoElbow/position", 1, + sub2 = nh.subscribe("mirte/servos/servoElbow/position", 1, rrbot_control::callbackJoint2); - sub3 = nh.subscribe("/mirte/servos/servoWrist/position", 1, + sub3 = nh.subscribe("mirte/servos/servoWrist/position", 1, rrbot_control::callbackJoint3); ROS_INFO_NAMED("rrbot_hw_interface", "RRBotHWInterface Ready."); @@ -105,20 +105,20 @@ RRBotHWInterface::RRBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model) void RRBotHWInterface::connectServices() { ROS_INFO_NAMED("rrbot_hw_interface", "Connecting to the services..."); - ros::service::waitForService("/mirte/set_servoRot_servo_angle", -1); - ros::service::waitForService("/mirte/set_servoShoulder_servo_angle", -1); - ros::service::waitForService("/mirte/set_servoElbow_servo_angle", -1); - ros::service::waitForService("/mirte/set_servoWrist_servo_angle", -1); + ros::service::waitForService("mirte/set_servoRot_servo_angle", -1); + ros::service::waitForService("mirte/set_servoShoulder_servo_angle", -1); + ros::service::waitForService("mirte/set_servoElbow_servo_angle", -1); + ros::service::waitForService("mirte/set_servoWrist_servo_angle", -1); { // Only mutex when actually writing to class vars. const std::lock_guard lock(this->service_clients_mutex); client0 = nh_.serviceClient( - "/mirte/set_servoRot_servo_angle", true); + "mirte/set_servoRot_servo_angle", true); client1 = nh_.serviceClient( - "/mirte/set_servoShoulder_servo_angle", true); + "mirte/set_servoShoulder_servo_angle", true); client2 = nh_.serviceClient( - "/mirte/set_servoElbow_servo_angle", true); + "mirte/set_servoElbow_servo_angle", true); client3 = nh_.serviceClient( - "/mirte/set_servoWrist_servo_angle", true); + "mirte/set_servoWrist_servo_angle", true); } ROS_INFO_NAMED("rrbot_hw_interface", "Connected to the services"); } diff --git a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py index d60bf111..9694e33a 100755 --- a/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py +++ b/mirte_telemetrix/scripts/ROS_telemetrix_aio_api.py @@ -22,7 +22,8 @@ from modules import MPU9250 -devices = rospy.get_param("/mirte/device") +# NOTE: This call was unused +# devices = rospy.get_param("mirte/device") current_soc = "???" # TODO: change to something better, but for now we communicate SOC from the powerwatcher to the Oled using a global @@ -91,7 +92,8 @@ def get_obj_value(s, obj, key, def_value=None): board_mapping = mappings.default -devices = rospy.get_param("/mirte/device") +# Moved the board_mapping setting to after intialization or ROS Node, since than relative names can be used. +devices = rospy.get_param("mirte/device") if devices["mirte"]["type"] == "pcb": board_mapping = mappings.pcb @@ -122,7 +124,7 @@ def get_obj_value(s, obj, key, def_value=None): def get_pin_numbers(component): - # devices = rospy.get_param("/mirte/device") + # devices = rospy.get_param("mirte/device") # device = devices[component["device"]] pins = {} if "connector" in component: @@ -199,16 +201,16 @@ async def publish(self, data): class KeypadMonitor(SensorMonitor): def __init__(self, board, sensor): - pub = rospy.Publisher("/mirte/keypad/" + sensor["name"], Keypad, queue_size=1) + pub = rospy.Publisher("mirte/keypad/" + sensor["name"], Keypad, queue_size=1) srv = rospy.Service( - "/mirte/get_keypad_" + sensor["name"], GetKeypad, self.get_data + "mirte/get_keypad_" + sensor["name"], GetKeypad, self.get_data ) super().__init__(board, sensor, pub, "keypad") self.last_debounce_time = 0 self.last_key = "" self.last_debounced_key = "" self.pressed_publisher = rospy.Publisher( - "/mirte/keypad/" + sensor["name"] + "_pressed", Keypad, queue_size=1 + "mirte/keypad/" + sensor["name"] + "_pressed", Keypad, queue_size=1 ) self.last_publish_value = Keypad() @@ -269,10 +271,10 @@ async def publish_data(self, data): class DistanceSensorMonitor(SensorMonitor): def __init__(self, board, sensor): self.pub = rospy.Publisher( - "/mirte/distance/" + sensor["name"], Range, queue_size=1, latch=True + "mirte/distance/" + sensor["name"], Range, queue_size=1, latch=True ) srv = rospy.Service( - "/mirte/get_distance_" + sensor["name"], GetDistance, self.get_data + "mirte/get_distance_" + sensor["name"], GetDistance, self.get_data ) super().__init__(board, sensor, self.pub, "distance") self.last_publish_value = Range() @@ -317,13 +319,13 @@ def publish_data(self, event=None): class DigitalIntensitySensorMonitor(SensorMonitor): def __init__(self, board, sensor): pub = rospy.Publisher( - "/mirte/intensity/" + sensor["name"] + "_digital", + "mirte/intensity/" + sensor["name"] + "_digital", IntensityDigital, queue_size=1, latch=True, ) srv = rospy.Service( - "/mirte/get_intensity_" + sensor["name"] + "_digital", + "mirte/get_intensity_" + sensor["name"] + "_digital", GetIntensityDigital, self.get_data, ) @@ -348,10 +350,10 @@ async def publish_data(self, data): class AnalogIntensitySensorMonitor(SensorMonitor): def __init__(self, board, sensor): pub = rospy.Publisher( - "/mirte/intensity/" + sensor["name"], Intensity, queue_size=100 + "mirte/intensity/" + sensor["name"], Intensity, queue_size=100 ) srv = rospy.Service( - "/mirte/get_intensity_" + sensor["name"], GetIntensity, self.get_data + "mirte/get_intensity_" + sensor["name"], GetIntensity, self.get_data ) super().__init__(board, sensor, pub, "intensity") self.last_publish_value = Intensity() @@ -376,13 +378,13 @@ async def publish_data(self, data): class EncoderSensorMonitor(SensorMonitor): def __init__(self, board, sensor): pub = rospy.Publisher( - "/mirte/encoder/" + sensor["name"], Encoder, queue_size=1, latch=True + "mirte/encoder/" + sensor["name"], Encoder, queue_size=1, latch=True ) srv = rospy.Service( - "/mirte/get_encoder_" + sensor["name"], GetEncoder, self.get_data + "mirte/get_encoder_" + sensor["name"], GetEncoder, self.get_data ) self.speed_pub = rospy.Publisher( - "/mirte/encoder_speed/" + sensor["name"], Encoder, queue_size=1, latch=True + "mirte/encoder_speed/" + sensor["name"], Encoder, queue_size=1, latch=True ) super().__init__(board, sensor, pub, "encoder") self.ticks_per_wheel = 20 @@ -446,10 +448,10 @@ def __init__(self, board, neo_obj): get_obj_value(self, neo_obj, "max_intensity", 50) get_obj_value(self, neo_obj, "default_color", 0x000000) server = rospy.Service( - f"/mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service + f"mirte/set_{self.name}_color_all", SetLEDValue, self.set_color_all_service ) server = rospy.Service( - f"/mirte/set_{self.name}_color_single", + f"mirte/set_{self.name}_color_single", SetSingleLEDValue, self.set_color_single_service, ) @@ -514,7 +516,7 @@ async def stop(self): async def start(self): await board.set_pin_mode_servo(self.pins["pin"], self.min_pulse, self.max_pulse) server = rospy.Service( - "/mirte/set_" + self.name + "_servo_angle", + "mirte/set_" + self.name + "_servo_angle", SetServoAngle, self.set_servo_angle_service, ) @@ -535,12 +537,12 @@ def __init__(self, board, motor_obj): async def start(self): server = rospy.Service( - "/mirte/set_" + self.name + "_speed", + "mirte/set_" + self.name + "_speed", SetMotorSpeed, self.set_motor_speed_service, ) sub = rospy.Subscriber( - "/mirte/motor_" + self.name + "_speed", Int32, self.callback + "mirte/motor_" + self.name + "_speed", Int32, self.callback ) def callback(self, data): @@ -744,7 +746,7 @@ def __init__( async def start(self): server = rospy.Service( - "/mirte/set_" + self.oled_obj["name"] + "_image", + "mirte/set_" + self.oled_obj["name"] + "_image", SetOLEDImage, self.set_oled_image_service, ) @@ -945,7 +947,7 @@ async def show_png(self, file): async def handle_set_led_value(req): - led = rospy.get_param("/mirte/led") + led = rospy.get_param("mirte/led") await analog_write( board, get_pin_numbers(led)["pin"], @@ -1034,8 +1036,8 @@ def handle_set_pin_value(req): def actuators(loop, board, device): servers = [] - if rospy.has_param("/mirte/oled"): - oleds = rospy.get_param("/mirte/oled") + if rospy.has_param("mirte/oled"): + oleds = rospy.get_param("mirte/oled") oleds = {k: v for k, v in oleds.items() if v["device"] == device} oled_id = 0 for oled in oleds: @@ -1049,18 +1051,18 @@ def actuators(loop, board, device): servers.append(loop.create_task(oled_obj.start())) # TODO: support multiple leds - if rospy.has_param("/mirte/led"): - led = rospy.get_param("/mirte/led") + if rospy.has_param("mirte/led"): + led = rospy.get_param("mirte/led") loop.run_until_complete( set_pin_mode_analog_output(board, get_pin_numbers(led)["pin"]) ) server = aiorospy.AsyncService( - "/mirte/set_led_value", SetLEDValue, handle_set_led_value + "mirte/set_led_value", SetLEDValue, handle_set_led_value ) servers.append(loop.create_task(server.start())) - if rospy.has_param("/mirte/motor"): - motors = rospy.get_param("/mirte/motor") + if rospy.has_param("mirte/motor"): + motors = rospy.get_param("mirte/motor") motors = {k: v for k, v in motors.items() if v["device"] == device} for motor in motors: motor_obj = {} @@ -1074,21 +1076,21 @@ def actuators(loop, board, device): rospy.loginfo("Unsupported motor interface (ddp, dp, or pp)") servers.append(loop.create_task(motor_obj.start())) - if rospy.has_param("/mirte/servo"): - servos = rospy.get_param("/mirte/servo") + if rospy.has_param("mirte/servo"): + servos = rospy.get_param("mirte/servo") servos = {k: v for k, v in servos.items() if v["device"] == device} for servo in servos: servo = Servo(board, servos[servo]) servers.append(loop.create_task(servo.start())) - if rospy.has_param("/mirte/neopixel"): - neopixel = rospy.get_param("/mirte/neopixel") + if rospy.has_param("mirte/neopixel"): + neopixel = rospy.get_param("mirte/neopixel") servers.append(loop.create_task(Neopixel(board, neopixel).start())) - if rospy.has_param("/mirte/modules"): - servers += add_modules(rospy.get_param("/mirte/modules"), device) + if rospy.has_param("mirte/modules"): + servers += add_modules(rospy.get_param("mirte/modules"), device) # Set a raw pin value - server = rospy.Service("/mirte/set_pin_value", SetPinValue, handle_set_pin_value) + server = rospy.Service("mirte/set_pin_value", SetPinValue, handle_set_pin_value) return servers @@ -1099,8 +1101,8 @@ def actuators(loop, board, device): def sensors(loop, board, device): tasks = [] max_freq = 30 - if rospy.has_param("/mirte/device/mirte/max_frequency"): - max_freq = rospy.get_param("/mirte/device/mirte/max_frequency") + if rospy.has_param("mirte/device/mirte/max_frequency"): + max_freq = rospy.get_param("mirte/device/mirte/max_frequency") # For now, we need to set the analog scan interval to teh max_freq. When we set # this to 0, we do get the updates from telemetrix as fast as possible. In that @@ -1129,22 +1131,22 @@ def sensors(loop, board, device): ) # initialze distance sensors - if rospy.has_param("/mirte/distance"): - distance_sensors = rospy.get_param("/mirte/distance") + if rospy.has_param("mirte/distance"): + distance_sensors = rospy.get_param("mirte/distance") distance_sensors = { k: v for k, v in distance_sensors.items() if v["device"] == device } for sensor in distance_sensors: distance_sensors[sensor]["max_frequency"] = max_freq distance_publisher = rospy.Publisher( - "/mirte/" + sensor, Range, queue_size=1, latch=True + "mirte/" + sensor, Range, queue_size=1, latch=True ) monitor = DistanceSensorMonitor(board, distance_sensors[sensor]) tasks.append(loop.create_task(monitor.start())) # Initialize intensity sensors - if rospy.has_param("/mirte/intensity"): - intensity_sensors = rospy.get_param("/mirte/intensity") + if rospy.has_param("mirte/intensity"): + intensity_sensors = rospy.get_param("mirte/intensity") intensity_sensors = { k: v for k, v in intensity_sensors.items() if v["device"] == device } @@ -1160,8 +1162,8 @@ def sensors(loop, board, device): tasks.append(loop.create_task(monitor.start())) # Initialize keypad sensors - if rospy.has_param("/mirte/keypad"): - keypad_sensors = rospy.get_param("/mirte/keypad") + if rospy.has_param("mirte/keypad"): + keypad_sensors = rospy.get_param("mirte/keypad") keypad_sensors = { k: v for k, v in keypad_sensors.items() if v["device"] == device } @@ -1171,8 +1173,8 @@ def sensors(loop, board, device): tasks.append(loop.create_task(monitor.start())) # Initialize encoder sensors - if rospy.has_param("/mirte/encoder"): - encoder_sensors = rospy.get_param("/mirte/encoder") + if rospy.has_param("mirte/encoder"): + encoder_sensors = rospy.get_param("mirte/encoder") encoder_sensors = { k: v for k, v in encoder_sensors.items() if v["device"] == device } @@ -1185,8 +1187,8 @@ def sensors(loop, board, device): # Get a raw pin value # TODO: this still needs to be tested. We are waiting on an implementation of ananlog_read() # on the telemetrix side - rospy.Service("/mirte/get_pin_value", GetPinValue, handle_get_pin_value) - # server = aiorospy.AsyncService('/mirte/get_pin_value', GetPinValue, handle_get_pin_value) + rospy.Service("mirte/get_pin_value", GetPinValue, handle_get_pin_value) + # server = aiorospy.AsyncService('mirte/get_pin_value', GetPinValue, handle_get_pin_value) # tasks.append(loop.create_task(server.start())) return tasks @@ -1245,7 +1247,7 @@ def __init__(self, servo_name, servo_obj, pca_update_func): async def start(self): await self.pca_update_func(self.pin, 0, 0) server = rospy.Service( - "/mirte/set_" + self.name + "_servo_angle", + "mirte/set_" + self.name + "_servo_angle", SetServoAngle, self.set_servo_angle_service, ) @@ -1382,14 +1384,14 @@ def __init__(self, board, module_name, module): self.shutdown_triggered = False self.turn_off_trigger_start_time = -1 self.ina_publisher = rospy.Publisher( - "/mirte/power/" + module_name, BatteryState, queue_size=1 + "mirte/power/" + module_name, BatteryState, queue_size=1 ) self.ina_publisher_used = rospy.Publisher( - f"/mirte/power/{module_name}/used", Int32, queue_size=1 + f"mirte/power/{module_name}/used", Int32, queue_size=1 ) self.used_energy = 0 # mah self.last_used_calc = time.time() - server = rospy.Service("/mirte/shutdown", SetBool, self.shutdown_service) + server = rospy.Service("mirte/shutdown", SetBool, self.shutdown_service) self.last_low_voltage = -1 @@ -1661,7 +1663,7 @@ async def shutdown_robot(self): # ]: # TODO: use the oled obj directly without hard-coded names # try: # set_image = rospy.ServiceProxy( - # f"/mirte/set_{oled_name}_image", SetOLEDImage + # f"mirte/set_{oled_name}_image", SetOLEDImage # ) # TODO: does not work, as it is async, calling async.... # set_image("text", "Shutting down") @@ -1725,20 +1727,20 @@ def __init__(self, servo_name, servo_obj, bus): async def start(self): server = rospy.Service( - "/mirte/set_" + self.name + "_servo_angle", + "mirte/set_" + self.name + "_servo_angle", SetServoAngle, self.set_servo_angle_service, ) rospy.Service( - "/mirte/set_" + self.name + "_servo_enable", + "mirte/set_" + self.name + "_servo_enable", SetBool, self.set_servo_enabled_service, ) rospy.Service( - f"/mirte/get_{self.name}_servo_range", GetRange, self.get_servo_range + f"mirte/get_{self.name}_servo_range", GetRange, self.get_servo_range ) self.publisher = rospy.Publisher( - f"/mirte/servos/{self.name}/position", + f"mirte/servos/{self.name}/position", ServoPosition, queue_size=1, latch=True, @@ -1826,7 +1828,7 @@ async def start(self): self.set_enabled = updaters["set_enabled"] self.set_enabled_all = updaters["set_enabled_all"] rospy.Service( - "/mirte/set_" + self.name + "_all_servos_enable", + "mirte/set_" + self.name + "_all_servos_enable", SetBool, self.set_all_servos_enabled, ) diff --git a/mirte_telemetrix/scripts/modules/MPU9250.py b/mirte_telemetrix/scripts/modules/MPU9250.py index 93dc71ec..c1f0b663 100644 --- a/mirte_telemetrix/scripts/modules/MPU9250.py +++ b/mirte_telemetrix/scripts/modules/MPU9250.py @@ -39,7 +39,7 @@ async def start(self): except Exception as e: pass self.imu_publisher = rospy.Publisher( - f"/mirte/{self.name}/imu", Imu, queue_size=1 + f"mirte/{self.name}/imu", Imu, queue_size=1 ) if "frame_id" in self.module: @@ -52,7 +52,7 @@ async def start(self): await self.board.sensors.add_mpu9250(self.i2c_port, self.callback) self.serv = rospy.Service( - f"/mirte/{self.name}/get_imu", + f"mirte/{self.name}/get_imu", GetIMU, self.get_imu_service, ) diff --git a/mirte_teleop/launch/teleop_holo_joy.launch b/mirte_teleop/launch/teleop_holo_joy.launch index d6fe098e..9a2bc462 100644 --- a/mirte_teleop/launch/teleop_holo_joy.launch +++ b/mirte_teleop/launch/teleop_holo_joy.launch @@ -14,6 +14,6 @@ type="teleop_node" output="screen"> - + diff --git a/mirte_teleop/launch/teleop_joy.launch b/mirte_teleop/launch/teleop_joy.launch index 347d3551..42e23e7c 100644 --- a/mirte_teleop/launch/teleop_joy.launch +++ b/mirte_teleop/launch/teleop_joy.launch @@ -9,6 +9,6 @@ type="teleop_node" output="screen"> - + diff --git a/mirte_teleop/launch/teleopkey.launch b/mirte_teleop/launch/teleopkey.launch index d7b1e6e9..0710aad2 100644 --- a/mirte_teleop/launch/teleopkey.launch +++ b/mirte_teleop/launch/teleopkey.launch @@ -6,7 +6,7 @@ type="teleop_twist_keyboard.py" output="screen"> - + diff --git a/mirte_teleop/scripts/joyArm.py b/mirte_teleop/scripts/joyArm.py index ab905662..d35e2a40 100755 --- a/mirte_teleop/scripts/joyArm.py +++ b/mirte_teleop/scripts/joyArm.py @@ -74,15 +74,15 @@ def talker(): global set_rot, set_sh, set_el, set_gr rospy.init_node("arm_control", anonymous=True) rospy.Subscriber("/joy", Joy, callback_joy, queue_size=1) - rospy.Subscriber("/mirte/servos/servoRot/position", ServoPosition, cb_rot) - rospy.Subscriber("/mirte/servos/servoShoulder/position", ServoPosition, cb_sh) - rospy.Subscriber("/mirte/servos/servoElbow/position", ServoPosition, cb_el) - rospy.Subscriber("/mirte/servos/servoGripper/position", ServoPosition, cb_gr) - - set_rot = rospy.ServiceProxy("/mirte/set_servoRot_servo_angle", SetServoAngle) - set_sh = rospy.ServiceProxy("/mirte/set_servoShoulder_servo_angle", SetServoAngle) - set_el = rospy.ServiceProxy("/mirte/set_servoElbow_servo_angle", SetServoAngle) - set_gr = rospy.ServiceProxy("/mirte/set_servoGripper_servo_angle", SetServoAngle) + rospy.Subscriber("mirte/servos/servoRot/position", ServoPosition, cb_rot) + rospy.Subscriber("mirte/servos/servoShoulder/position", ServoPosition, cb_sh) + rospy.Subscriber("mirte/servos/servoElbow/position", ServoPosition, cb_el) + rospy.Subscriber("mirte/servos/servoGripper/position", ServoPosition, cb_gr) + + set_rot = rospy.ServiceProxy("mirte/set_servoRot_servo_angle", SetServoAngle) + set_sh = rospy.ServiceProxy("mirte/set_servoShoulder_servo_angle", SetServoAngle) + set_el = rospy.ServiceProxy("mirte/set_servoElbow_servo_angle", SetServoAngle) + set_gr = rospy.ServiceProxy("mirte/set_servoGripper_servo_angle", SetServoAngle) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown():