Skip to content

Commit

Permalink
Update tf2 utils to support timestamps
Browse files Browse the repository at this point in the history
  • Loading branch information
@rolker
rolker committed Feb 14, 2024
1 parent ce3bec8 commit d448fd5
Show file tree
Hide file tree
Showing 5 changed files with 88 additions and 73 deletions.
9 changes: 0 additions & 9 deletions include/project11/gz4d_geo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,15 +51,6 @@ namespace gz4d
return !(i%2);
}

// inline bool IsTrue(std::string const &s)
// {
// std::string l = boost::algorithm::to_lower_copy(boost::algorithm::trim_copy(s));
// return (l == "true" || l == "yes" || l == "on" || l == "1" || l == "t" || l == "y");
// }

// template<typename T> inline T Nan(){return std::numeric_limits<T>::quiet_NaN();}
// template<typename T> inline bool IsNan(T value){return (boost::math::isnan)(value);}

/// Used by std::shared_ptr's to hold pointers it shouldn't auto-delete.
struct NullDeleter
{
Expand Down
2 changes: 1 addition & 1 deletion include/project11/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ class PID
Ki_ = msg->Ki;
Kd_ = msg->Kd;
windup_limit_ = msg->windup_limit;
upper_limit_ = msg->windup_limit;
upper_limit_ = msg->upper_limit;
lower_limit_ = msg->lower_limit;
}

Expand Down
17 changes: 11 additions & 6 deletions include/project11/tf2_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,28 @@ namespace project11
}
}

bool canTransform(std::string map_frame="map")
bool canTransform(std::string map_frame="map", ros::Time target_time = ros::Time(0), ros::Duration timeout = ros::Duration(0.5))
{
return buffer()->canTransform("earth", map_frame, ros::Time(0), ros::Duration(0.5));
return buffer()->canTransform("earth", map_frame, target_time, timeout);
}

geometry_msgs::Point wgs84_to_map(geographic_msgs::GeoPoint const &position, std::string map_frame="map")
geometry_msgs::Point wgs84_to_map(geographic_msgs::GeoPoint const &position, std::string map_frame="map", ros::Time target_time = ros::Time(0))
{
geometry_msgs::Point ret;
try
{
geometry_msgs::TransformStamped t = buffer()->lookupTransform(map_frame,"earth",ros::Time(0));
geometry_msgs::TransformStamped t = buffer()->lookupTransform(map_frame,"earth",target_time);
LatLongDegrees p_ll;
fromMsg(position, p_ll);
bool alt_is_nan = std::isnan(position.altitude);
if(alt_is_nan)
p_ll.altitude() = 0.0;
ECEF p_ecef = p_ll;
geometry_msgs::Point in;
toMsg(p_ecef, in);
tf2::doTransform(in,ret,t);
if(alt_is_nan)
ret.z = std::nan("");
}
catch (tf2::TransformException &ex)
{
Expand All @@ -47,12 +52,12 @@ namespace project11
return ret;
}

geographic_msgs::GeoPoint map_to_wgs84(geometry_msgs::Point const &point, std::string map_frame="map")
geographic_msgs::GeoPoint map_to_wgs84(geometry_msgs::Point const &point, std::string map_frame="map", ros::Time target_time = ros::Time(0))
{
geographic_msgs::GeoPoint ret;
try
{
geometry_msgs::TransformStamped t = buffer()->lookupTransform("earth",map_frame,ros::Time(0));
geometry_msgs::TransformStamped t = buffer()->lookupTransform("earth",map_frame, target_time);
geometry_msgs::Point out;
tf2::doTransform(point,out,t);
ECEF out_ecef;
Expand Down
22 changes: 11 additions & 11 deletions include/project11/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,24 +23,24 @@ namespace project11
return v3.length();
}

typedef gz4d::GeoPointLatLongDegrees LatLongDegrees;
typedef gz4d::GeoPointECEF ECEF;
typedef gz4d::Point<double> Point;
typedef gz4d::LocalENU ENUFrame;
using LatLongDegrees = gz4d::GeoPointLatLongDegrees;
using ECEF = gz4d::GeoPointECEF;
using Point = gz4d::Point<double>;
using ENUFrame = gz4d::LocalENU;

// angle types that do not wrap
typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::Unclamped> AngleDegrees;
typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::Unclamped> AngleRadians;
using AngleDegrees = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::Unclamped>;
using AngleRadians = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::Unclamped>;

// angle types that wrap at +/- half a circle
typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::ZeroCenteredPeriod> AngleDegreesZeroCentered;
typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::ZeroCenteredPeriod> AngleRadiansZeroCentered;
using AngleDegreesZeroCentered = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::ZeroCenteredPeriod>;
using AngleRadiansZeroCentered = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::ZeroCenteredPeriod>;

// angle types that wrap at 0 and full circle
typedef gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::PositivePeriod> AngleDegreesPositive;
typedef gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::PositivePeriod> AngleRadiansPositive;
using AngleDegreesPositive = gz4d::Angle<double, gz4d::pu::Degree, gz4d::rt::PositivePeriod>;
using AngleRadiansPositive = gz4d::Angle<double, gz4d::pu::Radian, gz4d::rt::PositivePeriod>;

typedef gz4d::geo::WGS84::Ellipsoid WGS84;
using WGS84 = gz4d::geo::WGS84::Ellipsoid;

template <typename A> void fromMsg(const A& a, LatLongDegrees &b)
{
Expand Down
111 changes: 65 additions & 46 deletions launch/robot_core.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,53 +5,72 @@
<arg name="default_nav_stack" default="true"/>
<arg name="bridge_name" default="$(arg namespace)"/>

<rosparam command="load" file="$(find project11)/config/robot.yaml" ns="$(arg namespace)"/>
<arg name="tf_prefix" default="$(arg namespace)"/>
<arg name="base_frame" value="$(arg tf_prefix)/base_link"/>
<arg name="map_frame" value="$(arg tf_prefix)/map"/>
<arg name="odom_frame" value="$(arg tf_prefix)/odom"/>

<!-- The udp_bridge_node is what sends and receives messages from select topics to the operator. -->
<node if="$(arg enableBridge)" pkg="udp_bridge" type="udp_bridge_node" name="udp_bridge" ns="$(arg namespace)">
<param name="port" value="$(arg localPort)"/>
<param name="name" value="$(arg bridge_name)"/>
</node>

<group ns="$(arg namespace)">

<rosparam command="load" file="$(find project11)/config/robot.yaml"/>

<!-- The udp_bridge_node is what sends and receives messages from select topics to the operator. -->
<node if="$(arg enableBridge)" pkg="udp_bridge" type="udp_bridge_node" name="udp_bridge">
<param name="port" value="$(arg localPort)"/>
<param name="name" value="$(arg bridge_name)"/>
</node>

<!-- Command bridge is used to robustly send operator commands over an unreliable connection. -->
<node pkg="command_bridge" type="command_bridge_receiver_node.py" name="command_bridge_receiver"/>

<!-- mru_transform Provides tf2 transforms from multiple gps and motion sensor sources. -->
<node pkg="mru_transform" type="mru_transform_node" name="mru_transform">
<param name="base_frame" value="$(arg base_frame)"/>
<param name="map_frame" value="$(arg map_frame)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<remap from="nav/position" to="project11/nav/position"/>
<remap from="nav/orientation" to="project11/nav/orientation"/>
<remap from="nav/velocity" to="project11/nav/velocity"/>
<remap from="nav/active_sensor" to="project11/nav/active_sensor"/>
</node>

<!-- Command bridge is used to robustly send operator commands over an unreliable connection. -->
<node pkg="command_bridge" type="command_bridge_receiver_node.py" name="command_bridge_receiver" ns="$(arg namespace)"/>

<!-- mru_transform Provides tf2 transforms from multiple gps and motion sensor sources. -->
<node pkg="mru_transform" type="mru_transform_node" name="mru_transform" ns="$(arg namespace)">
<param name="base_frame" value="$(arg namespace)/base_link"/>
<param name="map_frame" value="$(arg namespace)/map"/>
<param name="odom_frame" value="$(arg namespace)/odom"/>
<remap from="nav/position" to="project11/nav/position"/>
<remap from="nav/orientation" to="project11/nav/orientation"/>
<remap from="nav/velocity" to="project11/nav/velocity"/>
<remap from="nav/active_sensor" to="project11/nav/active_sensor"/>
</node>

<!-- helm_manager is the low level heart of project11. It manages which control messages get
sent to the robot based on piloting mode and reports the piloting mode and other status
as a heartbeat message. -->
<node pkg="helm_manager" type="helm_manager_node" name="helm_manager" ns="$(arg namespace)">
<remap from="out/helm" to="project11/control/helm"/>
<remap from="out/cmd_vel" to="project11/control/cmd_vel"/>
<remap from="heartbeat" to="project11/heartbeat"/>
<remap from="status/helm" to="project11/status/helm"/>
<remap from="piloting_mode" to="project11/piloting_mode"/>
<param name="piloting_mode_prefix" value="project11/piloting_mode/"/>
</node>

<!-- mission_manager handles commands from the operator and manages the task list sent to the navigator.-->
<node pkg="mission_manager" type="mission_manager_node.py" name="mission_manager" ns="$(arg namespace)">
<param name="map_frame" value="$(arg namespace)/map"/>
</node>

<!-- project11_navigation is the plugin based navigation stack that uses a list of tasks to plan trajectories
and generate commands to drive the robot. -->
<node pkg="project11_navigation" type="navigator" name="navigator" ns="$(arg namespace)">
<remap from="~/cmd_vel" to="project11/piloting_mode/autonomous/cmd_vel"/>
<remap from="~/enable" to="project11/piloting_mode/autonomous/active"/>
</node>
<include if="$(arg default_nav_stack)" file="$(find project11_navigation)/launch/default_nav_stack.launch">
<arg name="namespace" value="$(arg namespace)/navigator"/>
</include>
<!-- helm_manager is the low level heart of project11. It manages which control messages get
sent to the robot based on piloting mode and reports the piloting mode and other status
as a heartbeat message. -->
<node pkg="helm_manager" type="helm_manager_node" name="helm_manager">
<remap from="out/helm" to="project11/control/helm"/>
<remap from="out/cmd_vel" to="project11/control/cmd_vel"/>
<remap from="heartbeat" to="project11/heartbeat"/>
<remap from="status/helm" to="project11/status/helm"/>
<remap from="piloting_mode" to="project11/piloting_mode"/>
<param name="piloting_mode_prefix" value="project11/piloting_mode/"/>
</node>

<!-- mission_manager handles commands from the operator and manages the task list sent to the navigator.-->
<node pkg="mission_manager" type="mission_manager_node.py" name="mission_manager">
<param name="map_frame" value="$(arg map_frame)"/>
</node>

<!-- project11_navigation is the plugin based navigation stack that uses a list of tasks to plan trajectories
and generate commands to drive the robot. -->
<node pkg="project11_navigation" type="navigator" name="navigator">
<remap from="~/cmd_vel" to="project11/piloting_mode/autonomous/cmd_vel"/>
<remap from="~/enable" to="project11/piloting_mode/autonomous/active"/>
</node>
<node pkg="project11_navigation" type="occupancy_vector_map_from_geo" name="occupancy_vector_map_from_geo">
<remap from="~/input" to="project11/avoidance_map"/>
<remap from="~/output" to="project11/avoidance_map_local"/>
<param name="frame_id" value="$(arg map_frame)"/>
</node>
<node pkg="project11_navigation" type="occupancy_grid_from_vector_map" name="occupancy_grid_from_vector_map">
<remap from="~/input" to="project11/avoidance_map_local"/>
<remap from="~/output" to="project11/avoidance_grid"/>
</node>
<include if="$(arg default_nav_stack)" file="$(find project11_navigation)/launch/default_nav_stack.launch">
<arg name="namespace" value="navigator"/>
</include>

</group>

</launch>

0 comments on commit d448fd5

Please sign in to comment.