Skip to content

Fixed undefined reference errors #8

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

Open
mnksnz wants to merge 7 commits into
base: master
Choose a base branch
from
Open

Fixed undefined reference errors #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6701962
Fixed undefined reference errors
mnksnz May 27, 2019
739f046
Fixed aeplanner pkg errors using catkin_lint
mnksnz May 27, 2019
b6945da
Minor fix added missing build_export_depend
mnksnz May 27, 2019
bff5178
Added the frame id as map frame and modified the x, z values for UGVs
mnksnz Jul 18, 2019
93dd463
initial goal is 5 in x, framed id is odom
mnksnz Jul 29, 2019
0fb1475
Frame id odom, boundary for exploration reduced
mnksnz Jul 29, 2019
f0b5f72
Changed the rate for debugging, frame id is odom and added ros ok
mnksnz Jul 29, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 5 additions & 2 deletions aeplanner/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,15 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
visualization_msgs
message_generation
rqt_gui_cpp
rqt_gui
tf
tf2
genmsg
actionlib_msgs
actionlib
nodelet
std_msgs

kdtree
pigain
Expand Down Expand Up @@ -44,9 +48,8 @@ generate_messages(
)

catkin_package(
INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} ${OCTOMAP_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
LIBRARIES aeplanner ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES}
CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs tf kdtree
CATKIN_DEPENDS message_runtime roscpp geometry_msgs visualization_msgs tf kdtree actionlib_msgs std_msgs
)

include_directories(
Expand Down
10 changes: 10 additions & 0 deletions aeplanner/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,14 @@

<buildtool_depend>catkin</buildtool_depend>

<build_depend>genmsg</build_depend>
<build_depend>rqt_gui_cpp</build_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>octomap</build_depend>
<build_depend>rqt_gui</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>tf</build_depend>
Expand All @@ -23,6 +31,8 @@
<build_depend>pigain</build_depend>
<build_depend>dynamic_reconfigure</build_depend>

<run_depend>actionlib_msgs</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>tf</run_depend>
Expand Down
1 change: 1 addition & 0 deletions pigain/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@
<build_depend>geometry_msgs</build_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_depend>message_generation</build_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
Expand Down
8 changes: 4 additions & 4 deletions rpl_exploration/config/exploration.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,13 +22,13 @@ rrt/min_nodes: 100

# bounding box: necessary to limit the simulation
# scenario (smaller than actual gazebo scenario)
boundary/min: [-30.0, -30.0, 0.0]
boundary/max: [ 30.0, 30.0, 2.0]
boundary/min: [-10.0, -10.0, 0.0]
boundary/max: [ 10.0, 10.0, 1.0]

visualize_rays: false
visualize_tree: true
visualize_exploration_area: true

robot_frame: "fcu"
world_frame: "map"
robot_frame: "base_link"
world_frame: "odom"

8 changes: 4 additions & 4 deletions rpl_exploration/src/fly_to.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ namespace rpl_exploration {
<< goal->pose.pose.position.y << ", "
<< goal->pose.pose.position.z << ") ");

ros::Rate r(20);
ros::Rate r(2);
geometry_msgs::Point p = goal->pose.pose.position;

float distance_to_goal = 9001; // Distance is over 9000
Expand All @@ -52,8 +52,8 @@ namespace rpl_exploration {
ROS_INFO_STREAM("Publishing goal to (" << p.x << ", " << p.y << ", " << p.z << ") ");
pub_.publish(goal->pose);

listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
listener.waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform);

geometry_msgs::Point q;
q.x = (float)transform.getOrigin().x();
Expand Down Expand Up @@ -81,7 +81,7 @@ namespace rpl_exploration {
yaw_diff = fabs(atan2(sin(goal_yaw-current_yaw), cos(goal_yaw-current_yaw)));

r.sleep();
} while(distance_to_goal > 0.8 or yaw_diff > 0.6*M_PI);
} while(ros::ok() && (distance_to_goal > 0.8 or yaw_diff > 0.6*M_PI));


as->setSucceeded();
Expand Down
45 changes: 34 additions & 11 deletions rpl_exploration/src/rpl_exploration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,20 @@ int main(int argc, char** argv)
"local_position/pose");
double init_yaw = tf2::getYaw(init_pose->pose.orientation);
// Up 2 meters and then forward one meter
double initial_positions[8][4] = {
{ init_pose->pose.position.x, init_pose->pose.position.y,
init_pose->pose.position.z + 2.0, init_yaw },
{ init_pose->pose.position.x + 1.0 * std::cos(init_yaw),
init_pose->pose.position.y + 1.0 * std::sin(init_yaw),
init_pose->pose.position.z + 2.0, init_yaw },
};
// double initial_positions[8][4] = {
// { init_pose->pose.position.x, init_pose->pose.position.y,
// init_pose->pose.position.z + 2.0, init_yaw },
// { init_pose->pose.position.x + 1.0 * std::cos(init_yaw),
// init_pose->pose.position.y + 1.0 * std::sin(init_yaw),
// init_pose->pose.position.z + 2.0, init_yaw },
// };

// Modification for UGV to reduce z to zero
double initial_positions[8][4] = {
{ init_pose->pose.position.x + 5, init_pose->pose.position.y, 0.0, init_yaw },
{ init_pose->pose.position.x + 5 * std::cos(init_yaw),
init_pose->pose.position.y + 1.0 * std::sin(init_yaw), 0.0, init_yaw },
};

// This is the initialization motion, necessary that the known free space
// allows the planning of initial paths.
Expand All @@ -79,17 +86,24 @@ int main(int argc, char** argv)
for (int i = 0; i < 2; ++i)
{
rpl_exploration::FlyToGoal goal;
// Added header to support move base
goal.pose.header.stamp = ros::Time::now();
goal.pose.header.frame_id = "odom";
goal.pose.pose.position.x = initial_positions[i][0];
goal.pose.pose.position.y = initial_positions[i][1];
goal.pose.pose.position.z = initial_positions[i][2];
goal.pose.pose.orientation =
tf::createQuaternionMsgFromYaw(initial_positions[i][3]);
last_pose.pose = goal.pose.pose;

ROS_INFO_STREAM("Sending initial goal...");
last_pose.header.stamp = ros::Time::now();
last_pose.header.frame_id = "odom";
ROS_INFO_STREAM("Current position: (" << init_pose->pose.position.x << ", " << init_pose->pose.position.y << ", " << init_pose->pose.position.z << ") ");
ROS_INFO_STREAM("Sending initial goal..." << goal);
ac.sendGoal(goal);

ac.waitForResult(ros::Duration(0));

printf("Current State: %s\n", ac.getState().toString().c_str());
}

// Start planning: The planner is called and the computed path sent to the
Expand All @@ -104,14 +118,17 @@ int main(int argc, char** argv)
aeplanner::aeplannerGoal aep_goal;
aep_goal.header.stamp = ros::Time::now();
aep_goal.header.seq = iteration;
aep_goal.header.frame_id = "map";
aep_goal.header.frame_id = "odom";
aep_goal.actions_taken = actions_taken;
ROS_INFO_STREAM("Sending goal..." << aep_goal);
aep_ac.sendGoal(aep_goal);

while (!aep_ac.waitForResult(ros::Duration(0.05)))
{
pub.publish(last_pose);
ROS_INFO_STREAM("Not waiting for the result, sending last pose." << last_pose);
}
ROS_INFO_STREAM("Current result" << aep_ac.getResult());

ros::Duration fly_time;
if (aep_ac.getResult()->is_clear)
Expand All @@ -126,6 +143,9 @@ int main(int argc, char** argv)

last_pose.pose = goal_pose.pose;
rpl_exploration::FlyToGoal goal;
// Added header to support move base
goal.pose.header.stamp = ros::Time::now();
goal.pose.header.frame_id = "odom";
goal.pose = goal_pose;
ac.sendGoal(goal);

Expand All @@ -137,7 +157,7 @@ int main(int argc, char** argv)
{
rrtplanner::rrtGoal rrt_goal;
rrt_goal.start.header.stamp = ros::Time::now();
rrt_goal.start.header.frame_id = "map";
rrt_goal.start.header.frame_id = "odom";
rrt_goal.start.pose = last_pose.pose;
if (!aep_ac.getResult()->frontiers.poses.size())
{
Expand Down Expand Up @@ -167,6 +187,9 @@ int main(int argc, char** argv)

last_pose.pose = goal_pose;
rpl_exploration::FlyToGoal goal;
// Added header to support move base
goal.pose.header.stamp = ros::Time::now();
goal.pose.header.frame_id = "odom";
goal.pose.pose = goal_pose;
ac.sendGoal(goal);

Expand Down