Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

OGRID: add size param and visuals #278

Merged
merged 3 commits into from
Jun 28, 2017
Merged

OGRID: add size param and visuals #278

merged 3 commits into from
Jun 28, 2017

Conversation

RustyBamboo
Copy link
Member

  • publish blueview pings as a plane (point_cloud/plane)
  • param for waypointvalidity check size (sub_ogrid_size)
  • publish ogrid for desired waypoint (green if unoccupied, red if invalid) and publish ogrid for current waypoint (orange)
    screenshot from 2017-06-08 10-27-59
    screenshot from 2017-06-08 10-28-23

@RustyBamboo RustyBamboo force-pushed the waypoint_validity branch 2 times, most recently from b121d13 to 5c2eac5 Compare June 8, 2017 16:15
@RustyBamboo
OGRID: ignore points below dvl, add waypoint visuals
3dea811 
Signed-off-by: RustyBamboo <dvolya@gmail.com>
@RustyBamboo RustyBamboo changed the title OGRID: make ogrid center on sub; add size param and visuals OGRID: add size param and visuals Jun 9, 2017
@RustyBamboo
Copy link
Member Author

Reference to #277

DSsoto
DSsoto requested changes Jun 9, 2017
View reviewed changes
Copy link
Member

@DSsoto DSsoto left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd like your rationale for separate ogrid's and a couple requested changes.

legacy/c3_trajectory_generator/include/waypoint_validity.hpp Outdated
@@ -32,6 +39,10 @@ class WaypointValidity

// Usage: Store the reference to the previous ogrid in publisher
void ogrid_callback(const nav_msgs::OccupancyGridConstPtr &ogrid_map);
void odom_callback(const nav_msgs::OdometryConstPtr &odom);

void pub_size_ogrid(int d = 200);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should document these too.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

A more descriptive name like vehicle_odom_ogrid_pub would make the code more readable imo.

legacy/c3_trajectory_generator/src/waypoint_validity.cpp Outdated
@@ -70,9 +86,48 @@ std::pair<bool, WAYPOINT_ERROR_TYPE> WaypointValidity::is_waypoint_valid(const g
return std::make_pair(true, WAYPOINT_ERROR_TYPE::UNOCCUPIED);
}

void WaypointValidity::pub_size_ogrid(int d)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since the following two functions duplicate most of the code, I would suggest making the function take a raw pointer to a geometry_msgs::Pose object. If the pointer is null you use odom, otherwise you would use the pose object.

Add documentation in the header also.

legacy/c3_trajectory_generator/src/waypoint_validity.cpp Outdated
@@ -45,12 +45,17 @@ std::pair<bool, WAYPOINT_ERROR_TYPE> WaypointValidity::is_waypoint_valid(const g
return std::make_pair(false, WAYPOINT_ERROR_TYPE::NO_OGRID);
}

cv::Point where_sub = cv::Point(waypoint.position.x / ogrid_map_->info.resolution + ogrid_map_->info.width / 2,
waypoint.position.y / ogrid_map_->info.resolution + ogrid_map_->info.height / 2);
cv::Point center_of_ogird =
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

misspelled ogrid

legacy/c3_trajectory_generator/src/waypoint_validity.cpp
WaypointValidity::WaypointValidity(ros::NodeHandle &nh)
{
nh_ = &nh;
sub_ = nh_->subscribe<nav_msgs::OccupancyGrid>("/ogrid_pointcloud/ogrid", 1,
boost::bind(&WaypointValidity::ogrid_callback, this, _1));
nh_->param<double>("sub_ogrid_size", sub_ogrid_size_, 1.5);
pub_waypoint_ogrid_ = nh_->advertise<nav_msgs::OccupancyGrid>("/c3_trajectory_generator/waypoint_ogrid", 1, true);
pub_sub_ogrid_ = nh_->advertise<nav_msgs::OccupancyGrid>("/c3_trajectory_generator/sub_ogrid", 1, true);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What made you switch? Could you describe the benfit to having two ogrids?

I'm thinking it would be good to have an occupation type that encoded space being occupied by the sub but within the same ogrid.

legacy/c3_trajectory_generator/src/waypoint_validity.cpp Outdated
rosGrid.info.origin.position.x = waypoint.position.x - sub_ogrid_size_ / 2;
rosGrid.info.origin.position.y = waypoint.position.y - sub_ogrid_size_ / 2;
rosGrid.data = sub_ogrid_data;
if (d == 200)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You shouldn't branch on arbitrary magic constants like this. Consider using a flag parameter.

perception/sub8_pointcloud/include/Classification.hpp Outdated
@@ -40,7 +40,7 @@ class Classification
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(pcl::PointCloud<pcl::PointXYZI>::ConstPtr pointCloud);

// Usage: Find all the first occupied points in an expanding circle, then color the ogrid
void zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform);
void zonify(cv::Mat &mat_ogrid, float resolution, const tf::StampedTransform &transform, cv::Point &mat_origin);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should describe the parameters for this function.

perception/sub8_pointcloud/include/OGridGen.hpp Outdated
@@ -27,29 +27,39 @@

#include <Classification.hpp>

#include <mil_msgs/RangeStamped.h>
#include <ros_alarms/listener.hpp>

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This class needs better documentation.

perception/sub8_pointcloud/src/OGridGen.cpp
@@ -80,14 +96,15 @@ void OGridGen::publish_ogrid(const ros::TimerEvent &)
{
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This publish_ogrid function is getting a bit long, consider refactoring. For example, this loop could be replaced with a call to OGridGen::project_point_cloud_to_ogrid()

@RustyBamboo
Copy link
Member Author

I guess they're not really occupancy grids... they visualize the "buffer zone" for the current trajectory (orange) and the designated set waypoint (green for good, red for bad)...I believe this is similar to how we did it on Navigator.

I want to test out persistent ogrid tomorrow and will probably put that into this PR too.

@RustyBamboo
OGRID: implement permanance
054024f 
Signed-off-by: RustyBamboo <dvolya@gmail.com>
@DSsoto DSsoto merged commit 5d8dbdf into uf-mil:master Jun 28, 2017
@RustyBamboo RustyBamboo deleted the waypoint_validity branch July 7, 2017 11:26
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@DSsoto DSsoto DSsoto approved these changes

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@RustyBamboo @DSsoto