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.

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

Perception publish tags as a world pose #25

Merged
merged 19 commits into from
Jun 18, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ SpaceBeforeInheritanceColon: true
SpaceBeforeParens: ControlStatements
SpaceBeforeRangeBasedForLoopColon: false
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 0
SpacesBeforeTrailingComments: 1
SpacesInAngles: false
SpacesInCStyleCastParentheses: false
SpacesInContainerLiterals: false
Expand Down
30 changes: 15 additions & 15 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,9 @@ catkin_python_setup()
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
add_message_files(
FILES
MarkerInfo.msg
)
#add_message_files(
# FILES
#)

## Generate services in the 'srv' folder
# add_service_files(
Expand All @@ -89,10 +88,10 @@ add_message_files(
# )

## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
generate_messages(
DEPENDENCIES
std_msgs
)

################################################
## Declare ROS dynamic reconfigure parameters ##
Expand Down Expand Up @@ -125,7 +124,7 @@ generate_dynamic_reconfigure_options(
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES mrover
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
# DEPENDS system_lib
)

Expand All @@ -141,7 +140,8 @@ include_directories(
${OpenCV_INCLUDE_DIRS}
)

add_executable(aruco_detect src/perception/aruco_detect.cpp)
file(GLOB_RECURSE PERCEPTION_SOURCES "src/perception/*.cpp")
add_executable(aruco_detect ${PERCEPTION_SOURCES})
add_dependencies(aruco_detect ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(aruco_detect ${catkin_LIBRARIES} ${OpenCV_LIBS})

Expand Down Expand Up @@ -211,11 +211,11 @@ install(DIRECTORY launch/

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS differential_drive_plugin_6w
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(TARGETS differential_drive_plugin_6w
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
Expand Down
18 changes: 11 additions & 7 deletions launch/nav.launch
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,18 @@
-urdf -param robot_description
-model rover" />

<!-- Perception-->
<node name="aruco_detect" pkg="mrover" type="aruco_detect">
<param name="publish_images" value="true" />
<param name="fiducial_len" value="0.20" />
<param name="dictionary" value="0" />
</node>
<!--
==========
Perception
==========
-->
<node name="aruco_detect" pkg="mrover" type="aruco_detect" />

<!-- Navigation -->
<!--
===========
Navigation
===========
-->
<!-- This publishes the transforms of the course waypoints -->
<node name="debug_course_publisher" pkg="mrover" type="debug_course_publisher.py" />
qhdwight marked this conversation as resolved.
Show resolved Hide resolved

Expand Down
43 changes: 36 additions & 7 deletions launch/percep.launch
Original file line number Diff line number Diff line change
Expand Up @@ -25,12 +25,41 @@
-urdf -param robot_description
-model rover" />

<!-- Perception-->
<node name="aruco_detect" pkg="mrover" type="aruco_detect">
<param name="publish_images" value="true" />
<param name="fiducial_len" value="0.20" />
<param name="dictionary" value="0" /> <!-- DICT_4X4_50 -->
<remap from="camera" to="camera/color/image_raw" />
<remap from="camera_info" to="camera/color/camera_info" />
<!--
==========
Perception
==========
-->
<!-- <node name="aruco_detect" pkg="mrover" type="aruco_detect" /> -->

<!--
============
Localization
============
Frames in the free tree:
odom: The topmost global frame
base_link: The rover
-->
<node name="mrover_localization" pkg="mrover" type="localization.py" />

<rosparam command="load" file="$(find mrover)/params/ekf_localization.yaml" />

<!-- Fuses together GPS position and IMU orientation with an Extended Kalman Filter (EKF) -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_odom" clear_params="true">
<!-- Publications -->
<remap from="odometry/filtered" to="odometry/filtered" />
qhdwight marked this conversation as resolved.
Show resolved Hide resolved
</node>

<!-- Reads the GPS topic and convert it to cartesian based on a reference point-->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
<!-- Reference latitude/longitude to linearize from -->
<rosparam param="datum">[42.2, -83.7, 0.0, map, base_link]</rosparam>
<!-- Subscriptions -->
<remap from="imu/data" to="imu" />
<remap from="gps/fix" to="gps/fix" />
<remap from="odometry/filtered" to="odometry/filtered" />
<!-- Publications -->
<remap from="gps/filtered" to="gps/filtered" />
<remap from="odometry/gps" to="odometry/gps" />
</node>
</launch>
4 changes: 0 additions & 4 deletions msg/MarkerInfo.msg

This file was deleted.

8 changes: 4 additions & 4 deletions src/gazebo/differential_drive_6w.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -236,13 +236,13 @@ namespace gazebo {

double vr, va;

vr = x_; //myIface->data->cmdVelocity.pos.x;
va = -rot_;//myIface->data->cmdVelocity.yaw;
vr = x_; //myIface->data->cmdVelocity.pos.x;
qhdwight marked this conversation as resolved.
Show resolved Hide resolved
va = -rot_; //myIface->data->cmdVelocity.yaw;

//std::cout << "X: [" << x_ << "] ROT: [" << rot_ << "]" << std::endl;

// Changed motors to be always on, which is probably what we want anyway
enableMotors = true;//myIface->data->cmdEnableMotors > 0;
enableMotors = true; //myIface->data->cmdEnableMotors > 0;

//std::cout << enableMotors << std::endl;

Expand Down Expand Up @@ -317,4 +317,4 @@ namespace gazebo {

GZ_REGISTER_MODEL_PLUGIN(DiffDrivePlugin6W)

}// namespace gazebo
} // namespace gazebo
11 changes: 4 additions & 7 deletions src/gazebo/differential_drive_6w.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,7 @@
* Desc: ROS interface to a Position2d controller for a Differential drive.
* Author: Daniel Hewlett (adapted from Nathan Koenig)
*/
#ifndef DIFFDRIVE_PLUGIN_HH
#define DIFFDRIVE_PLUGIN_HH
#pragma once
qhdwight marked this conversation as resolved.
Show resolved Hide resolved

#include <map>

Expand Down Expand Up @@ -62,9 +61,9 @@ namespace gazebo {
~DiffDrivePlugin6W() override;

protected:
virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;

virtual void Reset();
void Reset() override;
qhdwight marked this conversation as resolved.
Show resolved Hide resolved

virtual void Update();

Expand Down Expand Up @@ -120,6 +119,4 @@ namespace gazebo {
event::ConnectionPtr updateConnection;
};

}// namespace gazebo

#endif
} // namespace gazebo
6 changes: 6 additions & 0 deletions src/perception/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
# [Perception](https://github.com/umrover/mrover-ros/wiki/Perception)

### Code Layout

- [aruco_detect.cpp](./aruco_detect.cpp) Mainly ROS node setup (topics, parameters, etc.)
- [aruco_processing.cpp](./aruco_detect.cpp) Processes inputs (camera feed and pointcloud) to estimate locations of the ArUco fiducials
Loading