Skip to content

Commit

Permalink
Clean up code
Browse files Browse the repository at this point in the history
  • Loading branch information
HoangGiang93 committed Jul 20, 2023
1 parent 255bb49 commit 4fc5cd7
Show file tree
Hide file tree
Showing 7 changed files with 16 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@ def __init__(self, **kwargs) -> None:
super().__init__(**kwargs)
self.__odom_publisher = rospy.Publisher("odom", Odometry, queue_size=0)
self.__odom_msg = Odometry()


def _init_request_meta_data(self) -> None:
super()._init_request_meta_data()
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/usr/bin/env python3

import rospy
from typing import List, Dict, Any
from typing import List, Dict
from multiverse_client.multiverse_ros_base import MultiverseRosBase


Expand Down Expand Up @@ -33,7 +33,7 @@ def start(self) -> None:
if not rospy.is_shutdown():
self._publish()
self.rate.sleep()

self._disconnect()

def _construct_rosmsg(self, response_meta_data_dict: Dict) -> None:
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,13 @@
#!/usr/bin/env python3

import rospy
from typing import List
from typing import Dict
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
from .ros_publisher import MultiverseRosPublisher
import numpy


class tf_publisher(MultiverseRosPublisher):
def __init__(self, **kwargs) -> None:
super().__init__(**kwargs)
Expand All @@ -21,22 +22,22 @@ def _init_request_meta_data(self) -> None:
super()._init_request_meta_data()
self._request_meta_data_dict["receive"][""] = ["position", "quaternion"]

def _construct_rosmsg(self, response_meta_data_dict: dict) -> None:
def _construct_rosmsg(self, response_meta_data_dict: Dict) -> None:
self.object_names = []
if response_meta_data_dict.get("receive") is None:
return

self.object_names = response_meta_data_dict["receive"].keys()
self.__tf_msgs = []

for object_name in self.object_names:
tf_data = response_meta_data_dict["receive"][object_name]
tf_msg = TransformStamped()
tf_msg.header.frame_id = self.__root_frame_id
tf_msg.header.stamp = rospy.Time.now()
tf_msg.header.seq = self.__seq
tf_msg.child_frame_id = object_name

tf_msg.transform.translation.x = tf_data["position"][0]
tf_msg.transform.translation.y = tf_data["position"][1]
tf_msg.transform.translation.z = tf_data["position"][2]
Expand All @@ -55,4 +56,4 @@ def _construct_rosmsg(self, response_meta_data_dict: dict) -> None:
self.__seq += 1

def _publish(self) -> None:
self.__tf_broadcaster.sendTransform(self.__tf_msgs)
self.__tf_broadcaster.sendTransform(self.__tf_msgs)
Original file line number Diff line number Diff line change
@@ -1,10 +1,9 @@
#!/usr/bin/env python3

from typing import Any
from typing import Dict
from .ros_service_server import MultiverseRosServiceServer
from multiverse_msgs.msg import ObjectAttribute, ObjectData
from multiverse_msgs.srv import Socket, SocketRequest, SocketResponse
import rospy


class query_data_service(MultiverseRosServiceServer):
Expand Down Expand Up @@ -70,7 +69,7 @@ def _bind_request_meta_data(self, request: SocketRequest) -> None:
continue
self._request_meta_data_dict["receive"][object_attribute.object_name].append(attribute_name)

def _bind_response(self, response_meta_data_dict: dict) -> SocketResponse:
def _bind_response(self, response_meta_data_dict: Dict) -> SocketResponse:
response = SocketResponse()
response.world = response_meta_data_dict["world"]
response.length_unit = response_meta_data_dict["length_unit"]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ def __init__(self, **kwargs) -> None:
self._topic_name = "/cmd_vel"
self._data_class = Twist

def _init_request_meta_data(self):
def _init_request_meta_data(self) -> None:
super()._init_request_meta_data()
self._request_meta_data_dict["send"] = {}
self._request_meta_data_dict["send"][self.__body] = ["relative_velocity"]
Expand All @@ -24,7 +24,7 @@ def _init_request_meta_data(self):
def _init_send_data(self) -> None:
self._send_data = [0.0] * 7

def _bind_send_data(self, twist_msg: Twist):
def _bind_send_data(self, twist_msg: Twist) -> None:
self._send_data[1] = twist_msg.linear.x
self._send_data[2] = twist_msg.linear.y
self._send_data[3] = twist_msg.linear.z
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,6 @@ def _subscriber_callback(self, data: Any) -> None:
self._communicate()
self._receive_data = self._get_receive_data()


def _init_send_data(self) -> None:
pass

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,11 @@ def start(self):
subscriber_thread = threading.Thread(target=subscriber.start)
subscriber_thread.start()
self.threads[subscriber] = subscriber_thread

publishers = rospy.get_param("multiverse/publishers", default={})
for publisher_name, publisher_prop in publishers.items():
if publisher_prop.get('port') is None:
rospy.logwarn(f'Ignore {publisher_name} because port is not found in rosparam')
if publisher_prop.get("port") is None:
rospy.logwarn(f"Ignore {publisher_name} because port is not found in rosparam")
continue
publisher_name += "_publisher"
exec(f"from multiverse_client.multiverse_publishers.{publisher_name} import {publisher_name}")
Expand Down

0 comments on commit 4fc5cd7

Please sign in to comment.