diff --git a/phoenix_description/urdf/phoenix.urdf b/phoenix_description/urdf/phoenix.urdf
index ef12138..1e565d1 100644
--- a/phoenix_description/urdf/phoenix.urdf
+++ b/phoenix_description/urdf/phoenix.urdf
@@ -393,6 +393,25 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py
index 3df74ea..a4c333d 100644
--- a/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py
+++ b/phoenix_gazebo/launch/include/robot_localization/robot_localization.launch.py
@@ -31,12 +31,8 @@
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
-<<<<<<< HEAD
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-=======
-from launch.substitutions import PathJoinSubstitution
->>>>>>> 81e19a3 (Fixed: PathJoinSubstitution updated teleop_ack_Rc included in gazeebo and robot)
def generate_launch_description():
pkg_share = get_package_share_directory('phoenix_robot')
diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml
index 6583660..4a158af 100644
--- a/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml
+++ b/phoenix_robot/config/navsat_transform/navsat_transform/navsat_transform.yaml
@@ -23,11 +23,11 @@ navsat_transform_node:
zero_altitude: false
# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
- broadcast_utm_transform: false
+ broadcast_utm_transform: true
# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform.
# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
- broadcast_utm_transform_as_parent_frame: false
+ broadcast_utm_transform_as_parent_frame: true
# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
# /gps/filtered. Defaults to false.
@@ -37,7 +37,7 @@ navsat_transform_node:
# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
# if your yaw data is based purely on integrated velocities. Defaults to false.
- use_odometry_yaw: false
+ use_odometry_yaw: true
# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
@@ -47,5 +47,6 @@ navsat_transform_node:
# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
# wait this might actually be usefully for translating over maps lol -AFB
+# NOTE WE
datum: [55.944904, -3.186693, 0.0]
diff --git a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml b/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml
deleted file mode 100644
index c7a6ac6..0000000
--- a/phoenix_robot/config/navsat_transform/navsat_transform/not_Sure.yaml
+++ /dev/null
@@ -1,51 +0,0 @@
-navsat_transform_node:
- ros__parameters:
-# Frequency of the main run loop
- frequency: 30.0
-
-# Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially
-# important if you have use_odometry_yaw set to true. Defaults to 0.
- delay: 3.0
-
-# PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame.
-# Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using
-# it.
-
-# If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it,
-# see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory.
- magnetic_declination_radians: 0.0
-
-# Your IMU's yaw, once the magentic_declination_radians value is added to it, should report 0 when facing east. If it
-# doesn't, enter the offset here. Defaults to 0.
- yaw_offset: 0.0
-
-# If this is true, the altitude is set to 0 in the output odometry message. Defaults to false.
- zero_altitude: false
-
-# If this is true, the transform world_frame->utm transform is broadcast for use by other nodes. Defaults to false.
- broadcast_utm_transform: false
-
-# If this is true, the utm->world_frame transform will be published instead of the world_frame->utm transform.
-# Note that broadcast_utm_transform still has to be enabled. Defaults to false.
- broadcast_utm_transform_as_parent_frame: false
-
-# If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as
-# /gps/filtered. Defaults to false.
- publish_filtered_gps: true
-
-# If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the
-# /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this!
-# The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw
-# if your yaw data is based purely on integrated velocities. Defaults to false.
- use_odometry_yaw: false
-
-# If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists,
-# navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message.
- wait_for_datum: false
-
-# Instead of using the first GPS location and IMU-based heading for the local-frame origin, users can specify the
-# origin (datum) using this parameter. The fields in the parameter represent latitude and longitude in decimal degrees,
-# and heading in radians. As navsat_transform_node assumes an ENU standard, a 0 heading corresponds to east.
-# wait this might actually be usefully for translating over maps lol -AFB
- datum: [55.944904, -3.186693, 0.0]
-
diff --git a/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml
new file mode 100755
index 0000000..89d003a
--- /dev/null
+++ b/phoenix_robot/config/phnx_io_ros/phnx_io_ros.yaml
@@ -0,0 +1,5 @@
+/phnx_io_ros:
+ ros__parameters:
+ kP: 1.0
+ kI: 1.0
+ kD: 0.1
\ No newline at end of file
diff --git a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml
index df48590..e32602e 100644
--- a/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml
+++ b/phoenix_robot/config/robot_localization/robot_localization_dual_ekf.yaml
@@ -23,9 +23,9 @@ ekf_filter_node_odom:
# vr vp vy
# ax ay az
- imu0: /phoenix/gps/imu #
+ imu0: /phoenix/imu
imu0_config: [false, false, false,
- true, true, true,
+ false, false, false,
false, false, false,
true, true, true,
true, true, true]
@@ -66,4 +66,4 @@ ekf_filter_node_map:
false, false, false,
false, false, false,
false, false, false]
- odom1_differential: false
\ No newline at end of file
+ odom1_differential: false
diff --git a/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml b/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml
index c266d88..93f0b63 100644
--- a/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml
+++ b/phoenix_robot/config/robot_localization/robot_localization_encoder_only.yaml
@@ -1,4 +1,7 @@
### ekf config file ###
+
+### THIS IS LAST YEARS EKF CONFIG LEAVE AS IS ###
+
ekf_filter_node:
ros__parameters:
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
diff --git a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml b/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml
deleted file mode 100644
index eabd9b4..0000000
--- a/phoenix_robot/config/robot_localization/robot_localization_gps_encoder_imu.yaml
+++ /dev/null
@@ -1,279 +0,0 @@
-
-### ekf config file ###
-ekf_filter_node:
- ros__parameters:
- # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
- # computation until it receives at least one message from one of the inputs. It will then run continuously at the
- # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
- frequency: 30.0
-
- # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
- # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
- # filter will generate new output. Defaults to 1 / frequency if not specified.
- #sensor_timeout: 1.0
-
- # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
- # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
- # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
- # by, for example, an IMU. Defaults to false if unspecified.
- two_d_mode: true
-
- # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
- # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
- # unspecified.
- transform_time_offset: 0.0
-
- # Use this parameter to provide specify how long the tf listener should wait for a transform to become available.
- # Defaults to 0.0 if unspecified.
- transform_timeout: 0.0
-
- # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
- # unhappy with any settings or data.
- print_diagnostics: true
-
- # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
- # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
- # effects on the performance of the node. Defaults to false if unspecified.
- debug: false
-
- # Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
- #debug_out_file: /path/to/debug/file.txt
-
- # Whether we'll allow old measurements to cause a re-publication of the updated state
- permit_corrected_publication: false
-
- # Whether to publish the acceleration state. Defaults to false if unspecified.
- publish_acceleration: false
-
- # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
- publish_tf: true
-
- # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
- # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
- # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
- # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
- # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
- # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
- # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
- # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
- # Here is how to use the following settings:
- # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
- # 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
- # odom_frame.
- # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
- # "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
- # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
- # from landmark observations) then:
- # 3a. Set your "world_frame" to your map_frame value
- # 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
- # estimation node from robot_localization! However, that instance should *not* fuse the global data.
- #map_frame: map # Defaults to "map" if unspecified
- odom_frame: odom # Defaults to "odom" if unspecified
- base_link_frame: base_footprint # Defaults to "base_link" if unspecified
- world_frame: odom # Defaults to the value of odom_frame if unspecified
-
- # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
- # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
- # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
- # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
- # default values, and must be specified.
- odom0: /odom_can # Just fuse vel from encoder
-
- # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
- # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
- # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
- # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
- # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
- # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
- # if unspecified, effectively making this parameter required for each sensor.
- odom0_config: [false, false, false, # x y z
- false, false, false, # r p y
- true, true, false, # vx vy vz
- false, false, false, # vr vp vy
- false, false, false] # ax ay az
-
- # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
- # the size of the subscription queue so that more measurements are fused.
- odom0_queue_size: 10
-
- # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
- # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
- # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
- # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
- # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
- # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
- # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
- # for twist measurements has no effect.
- odom0_differential: false
-
- # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
- # for all future measurements. While you can achieve the same effect with the differential paremeter, the key
- # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
- # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
- odom0_relative: false
-
- # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry.
- # Note: this is different from setting odom0_relative to true, as when child_frame is different from
- # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame.
- # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero
- # offset from base_link.
- odom0_pose_use_child_frame: false
-
- # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
- # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
- # numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not
- # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
- # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
- # the thresholds.
- #odom0_pose_rejection_threshold: 5.0
- #odom0_twist_rejection_threshold: 1.0
-
- # odom1: /odom_tracker TODO add when obj tracker updated
-
- # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
- # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
- # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
- # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
- # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
- # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
- # if unspecified, effectively making this parameter required for each sensor.
- # odom1_config: [false, false, false, # x y z
- # false, false, false, # r p y
- # true, false, false, # vx vy vz
- # false, false, false, # vr vp vy
- # false, false, false] # ax ay az
-
- # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
- # the size of the subscription queue so that more measurements are fused.
- # odom1_queue_size: 2
-
- # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
- # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
- # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
- # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
- # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
- # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
- # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
- # for twist measurements has no effect.
- # odom0_differential: false
-
- # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
- # for all future measurements. While you can achieve the same effect with the differential paremeter, the key
- # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
- # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
- # odom0_relative: false
-
- # [ADVANCED] Whether to use the starting pose of child_frame_id as the origin of odometry.
- # Note: this is different from setting odom0_relative to true, as when child_frame is different from
- # base_link_frame, the rotation of base_link will be coupled into the translation of child_frame.
- # Set to true for fusing secondary odometry sources that are rigidly connected to base_link but has a non-zero
- # offset from base_link.
- # odom0_pose_use_child_frame: false
-
- #imu0: /camera/mid/imu
- imu0: /phoenix/gps/imu
- imu0_config: [false, false, false,
- true, true, true,
- false, false, false,
- true, true, true,
- true, true, true]
- imu0_differential: false
- imu0_relative: false
- imu0_queue_size: 10
- #imu0_pose_rejection_threshold: 0.8
- #imu0_twist_rejection_threshold: 0.8
- #imu0_linear_acceleration_rejection_threshold: 0.8
-
- # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
- # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
- imu0_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo
-
- # imu1: /camera/right/imu
- # imu1_config: [false, false, false,
- # true, true, true,
- # false, false, false,
- # true, true, true,
- # true, true, true]
- # imu1_differential: false
- # imu1_relative: false
- # imu1_queue_size: 10
- # #imu0_pose_rejection_threshold: 0.8
- # #imu0_twist_rejection_threshold: 0.8
- # #imu0_linear_acceleration_rejection_threshold: 0.8
- #
- # # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
- # # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
- # imu1_remove_gravitational_acceleration: true # We need this for the oak-d and gazebo
-
- # [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
- # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
- # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
- # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
- # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
- # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
- # for the velocity variable in question, or decrease the variance of the variable in question in the measurement
- # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
- # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
- # predicition. Note that if an acceleration measurement for the variable in question is available from one of the
- # inputs, the control term will be ignored.
- # Whether or not we use the control input during predicition. Defaults to false.
- #use_control: false # cannot use because we are ackermann
-
- # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
- # false.
- #stamped_control: false
-
- # The last issued control command will be used in prediction for this period. Defaults to 0.2.
- #control_timeout: 0.2
-
- # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
- #control_config: [true, false, false, false, false, true]
-
- # Places limits on how large the acceleration term will be. Should match your robot's kinematics.
- #acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
-
- # Acceleration and deceleration limits are not always the same for robots.
- #deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
-
- # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
- # gains
- #acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
-
- # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
- # gains
- #deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
-
- # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
- # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
- # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
- # However, if users find that a given variable is slow to converge, one approach is to increase the
- # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
- # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
- # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
- # unspecified.
- # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support
- # both full specification or specification of only the diagonal values.
- process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0,
- 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015]
-
- # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
- # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
- # question. Users should take care not to use large values for variables that will not be measured directly. The values
- # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below
- # if unspecified. In this example, we specify only the diagonal of the matrix.
- initial_estimate_covariance: [1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9]
-
- use_sim_time: false
diff --git a/phoenix_robot/launch/common.launch.py b/phoenix_robot/launch/common.launch.py
index 21beed1..fb2751f 100644
--- a/phoenix_robot/launch/common.launch.py
+++ b/phoenix_robot/launch/common.launch.py
@@ -33,7 +33,7 @@ def generate_launch_description():
launch_arguments={'use_sim_time': use_sim_time}.items(),
)
- ekf = IncludeLaunchDescription(
+ robot_localization = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
os.path.join(pkg_phoenix_robot, 'launch')
, '/include/robot_localization/robot_localization.launch.py'
@@ -87,7 +87,7 @@ def generate_launch_description():
]),
launch_arguments={
'use_sim_time': use_sim_time,
- 'max_speed': '8.0',
+ 'max_speed': '4.0',
'min_speed': '0.5'
}.items(),
)
@@ -193,15 +193,15 @@ def generate_launch_description():
# Nodes
robot_state_controller,
state_publishers,
- ekf,
- # camera,
+ robot_localization,
+ camera,
pir,
pp,
- obj_detector_ai,
- obj_detector_cv,
- poly_plan,
- poly_plan_ai,
- # vectornav,
- # GPS_waypoints,
+ # obj_detector_ai,
+ # obj_detector_cv,
+ # poly_plan,
+ # poly_plan_ai,
+ vectornav,
+ GPS_waypoints,
teleop_ack_rc,
- ])
\ No newline at end of file
+ ])
diff --git a/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py b/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py
deleted file mode 100644
index 7a18b8f..0000000
--- a/phoenix_robot/launch/include/navsat_transform/navsat_transform.launch.py
+++ /dev/null
@@ -1,92 +0,0 @@
-# MIT License
-#
-# Copyright (c) 2021 Intelligent Systems Club
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-# SOFTWARE.
-
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import PathJoinSubstitution
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
- # 1. Define Arguments
- # We change the default to include .yaml so we can just join paths easily
- location_arg = DeclareLaunchArgument(
- 'location_file',
- default_value='dearborn.yaml',
- description='Name of location file (e.g., dearborn.yaml)'
- )
-
- use_sim_time_arg = DeclareLaunchArgument(
- 'use_sim_time',
- default_value='false',
- description='Use simulation time'
- )
-
- # 2. Setup Configuration Variables
- pkg_phoenix_robot = get_package_share_directory('phoenix_robot')
- location_file = LaunchConfiguration('location_file')
- use_sim_time = LaunchConfiguration('use_sim_time')
-
- # 3. Build Paths using Substitutions (No Python string manipulation needed)
- common_yaml_path = PathJoinSubstitution([
- pkg_phoenix_robot,
- 'config', 'navsat_transform', 'navsat_common.yaml'
- ])
-
- location_yaml_path = PathJoinSubstitution([
- pkg_phoenix_robot,
- 'config', 'navsat_transform', 'locations', location_file
- ])
-
- # 4. Define the Node
- navsat_node = Node(
- package='robot_localization', # Fixed package name
- executable='navsat_transform_node',
- name='navsat_transform_node',
- output='screen',
- parameters=[
- common_yaml_path, # Defaults
- location_yaml_path, # Location Specifics
- {'use_sim_time': use_sim_time}
- ],
- remappings=[
- ('imu/data', '/vectornav/imu'),
- ('gps/fix', '/gps/fix'),
- ('odometry/filtered', '/odometry/global'),
- ('odometry/gps', '/odometry/gps'),
- ('gps/filtered', '/gps/filtered')
- ]
- )
-
- return LaunchDescription([
- location_arg,
- use_sim_time_arg,
- navsat_node
- ])
\ No newline at end of file
diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py
index ac81094..4f336d4 100644
--- a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py
+++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py
@@ -1,22 +1,21 @@
import os
-from ament_index_python.packages import get_package_share_directory
+from launch_ros.substitutions import FindPackageShare
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
def generate_launch_description():
-
- pir = Node(package='phnx_io_ros',
- executable='phnx_io_ros',
- parameters=[{
- }]
- )
-
return LaunchDescription([
- # Launch Arguments
- pir
+ Node(
+ package='phnx_io_ros',
+ executable='phnx_io_ros',
+ parameters=[PathJoinSubstitution([
+ FindPackageShare('phoenix_robot'), 'config', 'phnx_io_ros', 'phnx_io_ros.yaml'])
+ ],
+ ),
])
diff --git a/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save
new file mode 100644
index 0000000..081c501
--- /dev/null
+++ b/phoenix_robot/launch/include/phnx_io_ros/phnx_io_ros.launch.py.save
@@ -0,0 +1,23 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ # This just tells the script where your new folder is
+ config = os.path.join(
+ get_package_share_directory('phoenix_robot'),
+ 'config/phnx_io_ros/phoenix_io_ros.yaml'
+ )
+
+ return LaunchDescription([
+ Node(
+ package='phnx_io_ros',
+ executable='phnx_io_ros',
+ name='phnx_io_ros',
+ parameters=[config],
+ output='screen'
+ )
+ ])
diff --git a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py
index a5b75c3..423d551 100644
--- a/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py
+++ b/phoenix_robot/launch/include/robot_localization/robot_localization.launch.py
@@ -59,7 +59,7 @@ def generate_launch_description():
name='ekf_filter_node_map',
output='screen',
parameters=[rl_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}],
- remappings=[('/odometry/filtered', '/odometry/global')]
+ remappings=[('/odometry/filtered', '/odometry/gps/filter')] # TODO make this '/odom_global/filtered' and change in GPS
)
# NAVSAT TRANSFORM NODE
@@ -67,14 +67,18 @@ def generate_launch_description():
navsat_transform = Node(
package='robot_localization',
executable='navsat_transform_node',
- name='navsat_transform',
+ name='navsat_transform_node',
output='screen',
- parameters=[navsat_config, {'use_sim_time': LaunchConfiguration('use_sim_time')}],
+ parameters=[navsat_config,
+ {'use_sim_time': LaunchConfiguration('use_sim_time'),
+ 'yaw_offset': LaunchConfiguration('yaw_offset')
+ }],
remappings=[
('imu', '/phoenix/imu'), # Data from VectorNav
('gps/fix', '/phoenix/navsat'), # Data from VectorNav
('odometry/filtered', '/odom'), # Input from LOCAL EKF
- ('odometry/gps', '/odometry/gps') # Output to GLOBAL EKF
+ ('odometry/gps', '/odometry/navsat_gps'), # Output to GLOBAL EKF
+ ('gps/filtered', 'gps/filtered'), # Extra gps/filtered sensor_msgs/NavSatFix
]
)
return LaunchDescription([
@@ -82,6 +86,10 @@ def generate_launch_description():
DeclareLaunchArgument('use_sim_time',
default_value='true',
description='Use simulation clock if true'),
+ DeclareLaunchArgument('yaw_offset',
+ default_value='-0.105', # flaot for dearborn, needto change for purdue
+ description='magnetic_declination_radians paramter'),
+
# Nodes
local_ekf,
global_ekf,
diff --git a/phoenix_robot/launch/utilibot.launch.py b/phoenix_robot/launch/utilibot.launch.py
index 201eb71..582f496 100644
--- a/phoenix_robot/launch/utilibot.launch.py
+++ b/phoenix_robot/launch/utilibot.launch.py
@@ -23,7 +23,7 @@ def generate_launch_description():
use_wheel = LaunchConfiguration('use_wheel', default='false')
max_braking_speed = LaunchConfiguration('max_braking_speed', default='-4.0')
- max_throttle_speed = LaunchConfiguration('max_throttle_speed', default='4.0')
+ max_throttle_speed = LaunchConfiguration('max_throttle_speed', default='2.0')
max_steering_rad = LaunchConfiguration('max_steering_rad', default='0.2733')
wheelbase = LaunchConfiguration('wheelbase', default='1.08')
@@ -75,7 +75,7 @@ def generate_launch_description():
default_value='-6.0',
description='Maximum braking speed'),
DeclareLaunchArgument('max_throttle_speed',
- default_value='6.0',
+ default_value='2.0',
description='Maximum throttle speed'),
DeclareLaunchArgument('wheelbase',
default_value='1.08',