-
Notifications
You must be signed in to change notification settings - Fork 5
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
20d1a5a
commit 754640a
Showing
16 changed files
with
143 additions
and
70 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
13 changes: 3 additions & 10 deletions
13
catkin_ws/src/autominy/launch/communication/Communication.launch
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,14 +1,7 @@ | ||
| <launch> | ||
| <arg name="live" description="Whether to instantiate a live system (true) or a bag-based one (false)" /> | ||
|
|
||
| <!-- Relay into communication ns --> | ||
| <!--<include file="$(find-pkg-share autominy)/launch/communication/Relay.launch"> | ||
| <arg name="live" value="$(var live)" /> | ||
| </include>--> | ||
|
|
||
| <!-- Multimaster communication --> | ||
| <!--<include file="$(find-pkg-share autominy)/launch/communication/Multimaster.launch"> | ||
| <arg name="live" value="$(var live)" /> | ||
| </include>--> | ||
|
|
||
| <node name="gps" exec="gps.py" pkg="road_marking_localization"> | ||
| <param name="id" value="$(env ROS_GPS_MARKER 999)"/> | ||
| </node> | ||
| </launch> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,55 @@ | ||
| #!/usr/bin/python3 | ||
|
|
||
| import rclpy | ||
| import roslibpy | ||
|
|
||
| from rclpy.node import Node | ||
| from rclpy.time import Time | ||
| from rclpy.executors import MultiThreadedExecutor | ||
|
|
||
| from nav_msgs.msg import Odometry | ||
|
|
||
| class GPSClient(Node): | ||
| def __init__(self): | ||
| super().__init__('gps_client') | ||
| self.id = self.declare_parameter('id', 16) | ||
| self.pub = self.create_publisher(Odometry, '/sensors/odometry/gps', 10) | ||
| client = roslibpy.Ros(host='192.168.43.2', port=9090) | ||
| client.run() | ||
| listener = roslibpy.Topic(client, '/communication/gps/' + str(self.get_parameter('id').value), 'nav_msgs/Odometry') | ||
| listener.subscribe(self.on_gps) | ||
|
|
||
| def on_gps(self, message): | ||
| odom = Odometry() | ||
| odom.header.stamp = self.get_clock().now().to_msg(); | ||
| odom.header.frame_id = message['header']['frame_id'] | ||
| odom.child_frame_id = message['child_frame_id'] | ||
| odom.pose.pose.position.x = message['pose']['pose']['position']['x'] | ||
| odom.pose.pose.position.y = message['pose']['pose']['position']['y'] | ||
| odom.pose.pose.position.z = message['pose']['pose']['position']['z'] | ||
| odom.pose.pose.orientation.x = message['pose']['pose']['orientation']['x'] | ||
| odom.pose.pose.orientation.y = message['pose']['pose']['orientation']['y'] | ||
| odom.pose.pose.orientation.z = message['pose']['pose']['orientation']['z'] | ||
| odom.pose.pose.orientation.w = message['pose']['pose']['orientation']['w'] | ||
| odom.twist.twist.linear.x = message['twist']['twist']['linear']['x'] | ||
| odom.twist.twist.linear.y = message['twist']['twist']['linear']['y'] | ||
| odom.twist.twist.linear.z = message['twist']['twist']['linear']['z'] | ||
| odom.twist.twist.angular.x = message['twist']['twist']['angular']['x'] | ||
| odom.twist.twist.angular.y = message['twist']['twist']['angular']['y'] | ||
| odom.twist.twist.angular.z = message['twist']['twist']['angular']['z'] | ||
| self.pub.publish(odom) | ||
|
|
||
| print(message) | ||
|
|
||
| def main(args=None): | ||
| rclpy.init(args=args) | ||
| gps_client = GPSClient() | ||
| executor = MultiThreadedExecutor() | ||
| executor.add_node(gps_client) | ||
|
|
||
| rclpy.spin(gps_client) | ||
| rclpy.shutdown() | ||
|
|
||
|
|
||
| if __name__ == '__main__': | ||
| main() |
69 changes: 47 additions & 22 deletions
69
catkin_ws/src/road_marking_localization/scripts/gps_initialpose.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,50 +1,75 @@ | ||
| #! /usr/bin/python3 | ||
| #!/usr/bin/python3 | ||
|
|
||
| import rospy | ||
| import rclpy | ||
| import math | ||
| from tf.transformations import euler_from_quaternion | ||
| import numpy as np | ||
|
|
||
| from nav_msgs.msg import Odometry | ||
| from geometry_msgs.msg import PoseWithCovarianceStamped | ||
| from rclpy.node import Node | ||
|
|
||
|
|
||
| def euler_from_quaternion(x, y, z, w): | ||
| sinr_cosp = 2 * (w * x + y * z) | ||
| cosr_cosp = 1 - 2 * (x * x + y * y) | ||
| roll = np.arctan2(sinr_cosp, cosr_cosp) | ||
|
|
||
| sinp = 2 * (w * y - z * x) | ||
| pitch = np.arcsin(sinp) | ||
|
|
||
| siny_cosp = 2 * (w * z + x * y) | ||
| cosy_cosp = 1 - 2 * (y * y + z * z) | ||
| yaw = np.arctan2(siny_cosp, cosy_cosp) | ||
|
|
||
| class GPSInitialpose: | ||
| return roll, pitch, yaw | ||
|
|
||
| class GPSInitialpose(Node): | ||
|
|
||
| def __init__(self): | ||
| self.initialpose_pub = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=1) | ||
| self.gps_sub = rospy.Subscriber("gps", Odometry, self.on_gps, queue_size=1) | ||
| self.localization_sub = rospy.Subscriber("localization", Odometry, self.on_localization, queue_size=1) | ||
| super().__init__("gps_initialpose") | ||
| self.initialpose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) | ||
| self.gps_sub = self.create_subscription(Odometry, "/sensors/odometry/gps", self.on_gps, 1) | ||
| self.localization_sub = self.create_subscription(Odometry, "/sensors/localization/filtered_map", self.on_localization, 1) | ||
| self.localization = None | ||
| self.tolerance_xy = rospy.get_param("tolerance_xy", 0.4) | ||
| self.tolerance_yaw = rospy.get_param("tolerance_yaw", 1.5) | ||
| self.last_correction = rospy.Time.now() | ||
| self.declare_parameter("tolerance_xy", 0.4) | ||
| self.declare_parameter("tolerance_yaw", 1.5) | ||
| self.tolerance_xy = self.get_parameter("tolerance_xy").value | ||
| self.tolerance_yaw = self.get_parameter("tolerance_yaw").value | ||
| self.last_correction = self.get_clock().now() | ||
|
|
||
| def on_localization(self, msg): | ||
| self.localization = msg | ||
|
|
||
| def on_gps(self, msg): | ||
| if self.localization and (rospy.Time.now() - self.last_correction).to_sec() > 3.0: | ||
| if self.localization and (self.get_clock().now() - self.last_correction).nanoseconds / 1e9 > 3.0: | ||
| distance = math.sqrt((self.localization.pose.pose.position.x - msg.pose.pose.position.x) ** 2.0 + ( | ||
| self.localization.pose.pose.position.y - msg.pose.pose.position.y) ** 2.0) | ||
| yaw_localization = euler_from_quaternion((self.localization.pose.pose.orientation.x, | ||
| self.localization.pose.pose.orientation.y, | ||
| self.localization.pose.pose.orientation.z, | ||
| self.localization.pose.pose.orientation.w))[2] | ||
| yaw_localization = euler_from_quaternion(self.localization.pose.pose.orientation.x, | ||
| self.localization.pose.pose.orientation.y, | ||
| self.localization.pose.pose.orientation.z, | ||
| self.localization.pose.pose.orientation.w)[2] | ||
| yaw_gps = euler_from_quaternion( | ||
| (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, | ||
| msg.pose.pose.orientation.w))[2] | ||
| msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, | ||
| msg.pose.pose.orientation.w)[2] | ||
| delta_yaw = math.fabs(yaw_localization - yaw_gps) | ||
|
|
||
| if distance > self.tolerance_xy: # or delta_yaw > self.tolerance_yaw: | ||
| print (distance, delta_yaw) | ||
| pose = PoseWithCovarianceStamped() | ||
| pose.pose = msg.pose | ||
| pose.header.frame_id = "map" | ||
| pose.header.stamp = rospy.Time.now() | ||
| pose.header.stamp = self.get_clock().now().to_msg() | ||
| self.initialpose_pub.publish(pose) | ||
| self.last_correction = rospy.Time.now() | ||
| self.last_correction = self.get_clock().now() | ||
|
|
||
| def main(args=None): | ||
| rclpy.init(args=args) | ||
|
|
||
| node = GPSInitialpose() | ||
| rclpy.spin(node) | ||
| node.destroy_node() | ||
| rclpy.shutdown() | ||
|
|
||
|
|
||
| if __name__ == '__main__': | ||
| rospy.init_node('gps_initialpose') | ||
| GPSInitialpose() | ||
| rospy.spin() | ||
| main() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters