Skip to content

Commit

Permalink
Merged with demo_main.
Browse files Browse the repository at this point in the history 
Squashed commit of the following:

commit 3ec31e4
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 15:00:49 2021 -0400

    resource localiser out of range fix

commit fb7c173
Merge: ced51ce e0b5d12
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 13:36:15 2021 -0400

    Merge branch 'eli_demo' into demo_main

commit e0b5d12
Author: Eli Benevedes <[email protected]>
Date:   Fri Mar 26 12:35:46 2021 -0400

    Scout state machine works with resource localiser

    Previously, we were incorrectly checking if the nav client was done in LOCATE, instead of the resource localiser client. Fixed.

    Backed off BASE_SPEED from 0.75 to 0.6

    Ignored duplicate transforms in publish cheat odom.

commit ced51ce
Merge: 8864c30 cae20e4
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 08:41:06 2021 -0400

    Merge branch 'mahi_eli_main' into demo_main

commit 8864c30
Merge: fd674d8 62c248b
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 08:05:22 2021 -0400

    Merge branch 'chintan_rohan' into demo_main

commit fd674d8
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 07:45:34 2021 -0400

    Compilation fix after merge

commit 71ca088
Merge: 3fd213f 528ecee
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 07:41:41 2021 -0400

    Merge branch 'mahi_main' into demo_main

commit 528ecee
Merge: c82da82 1683d9e
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 07:38:33 2021 -0400

    Merge branch 'eli_fork' into mahi_main

commit 3fd213f
Merge: 4822550 dbd394c
Author: Ashay Aswale <[email protected]>
Date:   Fri Mar 26 07:24:25 2021 -0400

    Merge branch 'forked_main' into demo_main

commit 62c248b
Author: chintujaguar <[email protected]>
Date:   Fri Mar 26 05:03:03 2021 +0000

    Updated excavator nodes with target position and smoother movement of joints

commit cae20e4
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 20:15:48 2021 -0400

    debug--it works!

commit 1683d9e
Merge: 49cd687 4822550
Author: Eli Benevedes <[email protected]>
Date:   Thu Mar 25 19:31:05 2021 -0400

    Merge branch 'main' of github.com:WPI-NASA-SRC-P2/capricorn_competition_round into main

commit 49cd687
Author: Eli Benevedes <[email protected]>
Date:   Thu Mar 25 19:28:52 2021 -0400

    Scout state machine works!

commit 5911c84
Author: chintujaguar <[email protected]>
Date:   Thu Mar 25 23:01:22 2021 +0000

    updated action server client nodes with x,y target for shoulder yaw joint

commit 0d00e4f
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 18:03:07 2021 -0400

    print

commit 87b71e1
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:59:43 2021 -0400

    threshold change

commit c3b0dea
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:56:47 2021 -0400

    default velocities

commit 6fcf184
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:52:09 2021 -0400

    fix width

commit a693411
Author: WaliaRohan <[email protected]>
Date:   Thu Mar 25 17:51:23 2021 -0400

    Made preliminary changes to Excavator action file, client and server

commit f474b9c
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:50:41 2021 -0400

    curly braces

commit e39722a
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:50:15 2021 -0400

    write callback, specify object as repair station

commit f2d05c0
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:43:50 2021 -0400

    nav vision

commit e61ed33
Merge: 5c6ffdc 41e6cba
Author: MahimanaGIT <[email protected]>
Date:   Thu Mar 25 17:12:59 2021 -0400

    updating nav vision

commit 5c6ffdc
Author: MahimanaGIT <[email protected]>
Date:   Thu Mar 25 17:10:48 2021 -0400

    updating navigation vision

commit 41e6cba
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:07:14 2021 -0400

    write callback function

commit de5f41b
Author: chrisdemaio <[email protected]>
Date:   Thu Mar 25 17:02:49 2021 -0400

    remove errors/unnecessary

commit d4fb36b
Author: MahimanaGIT <[email protected]>
Date:   Thu Mar 25 16:56:03 2021 -0400

    Adding nav vision/ eli main

commit 16cfaaa
Merge: fff20e7 825bbc7
Author: WaliaRohan <[email protected]>
Date:   Thu Mar 25 16:45:48 2021 -0400

    Merge branch 'rocinante_main' into Rohan
    Merging with rocinante_main to reflect changes made to excavator code.

commit 2805904
Author: Eli Benevedes <[email protected]>
Date:   Thu Mar 25 12:55:43 2021 -0400

    Removed extra comments and loud debugs

commit 299a897
Author: Eli Benevedes <[email protected]>
Date:   Thu Mar 25 12:53:58 2021 -0400

    Matching final orientation works

commit dbd394c
Author: Ashay Aswale <[email protected]>
Date:   Thu Mar 25 01:10:40 2021 -0400

    compilation error fix

commit da66e59
Author: Ashay Aswale <[email protected]>
Date:   Thu Mar 25 01:00:20 2021 -0400

    Can set orientation in resource_localiser.

commit 568b46b
Author: Eli Benevedes <[email protected]>
Date:   Wed Mar 24 23:55:49 2021 -0400

    Nav actionlib server, tests, and scout state machine

    Actionlib server (and nodes in this repo interacting with it) now depend on drive_mode, which is a NAV_TYPE in common_names.h. New dispatch has not been tested.
    Added tests to operations to test NavigationAlgo::changeInPosition. Tests have not been tested
    Implemented scout state machine states used for the demo. Have not tested.

commit c82da82
Author: MahimanaGIT <[email protected]>
Date:   Wed Mar 24 23:27:42 2021 -0400

    Updating navigation vision cpp

commit 20963a8
Author: chrisdemaio <[email protected]>
Date:   Wed Mar 24 23:12:47 2021 -0400

    more variable declarations, include files

commit 0ce9cba
Author: chrisdemaio <[email protected]>
Date:   Wed Mar 24 23:09:35 2021 -0400

    variable declarations

commit 7eacda5
Author: chrisdemaio <[email protected]>
Date:   Wed Mar 24 23:05:16 2021 -0400

    Fix string declaration

commit aa17e5f
Author: MahimanaGIT <[email protected]>
Date:   Wed Mar 24 23:00:00 2021 -0400

    Adding navigation to Cmake operations

commit 8174e96
Author: MahimanaGIT <[email protected]>
Date:   Wed Mar 24 22:54:31 2021 -0400

    Resolving merge conflict

commit 0485293
Merge: 7ad2ff6 d09b65e
Author: Eli Benevedes <[email protected]>
Date:   Wed Mar 24 15:23:15 2021 -0400

    Merge remote-tracking branch 'ashay/main' into main

commit d09b65e
Author: Ashay Aswale <[email protected]>
Date:   Wed Mar 24 12:25:09 2021 -0400

    scout SM skeleton

commit 7ad2ff6
Merge: a954090 06e9214
Author: Eli Benevedes <[email protected]>
Date:   Wed Mar 24 12:05:18 2021 -0400

    Merged from main repo'

commit a954090
Author: Eli Benevedes <[email protected]>
Date:   Wed Mar 24 11:55:26 2021 -0400

    Small style changes

commit e88ef7d
Merge: 1a28576 06e9214
Author: Ashay Aswale <[email protected]>
Date:   Wed Mar 24 11:38:24 2021 -0400

    Merge branch 'main' into forked_main

commit 825bbc7
Merge: f509636 c7dcd93
Author: Chintan Desai <[email protected]>
Date:   Wed Mar 24 00:30:35 2021 -0400

    Merge branch 'main' into rocinante_main

commit f509636
Author: chintujaguar <[email protected]>
Date:   Wed Mar 24 00:24:53 2021 -0400

    updated Excavator.action file to not rely on wrist position for deciding task

commit 9b9fdf8
Author: chintujaguar <[email protected]>
Date:   Tue Mar 23 23:57:38 2021 -0400

    tested and commented excavator action nodes

commit e30d014
Merge: a89c927 89f1e47
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 23:08:52 2021 -0400

    Merge branch 'main' of https://github.com/MahimanaGIT/capricorn_competition_round into main

commit a89c927
Merge: 5f34454 c7dcd93
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 23:08:34 2021 -0400

    Merging commit

commit 89f1e47
Author: chrisdemaio <[email protected]>
Date:   Tue Mar 23 22:40:07 2021 -0400

    Add some comments

commit 6a78229
Author: chrisdemaio <[email protected]>
Date:   Tue Mar 23 22:35:13 2021 -0400

    Change "size" to "height"

commit 9abf44c
Author: chrisdemaio <[email protected]>
Date:   Tue Mar 23 22:30:44 2021 -0400

    Generalize for any object

commit a2ed691
Author: chrisdemaio <[email protected]>
Date:   Tue Mar 23 22:24:20 2021 -0400

    Fill in objects_callback

commit 5f34454
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 21:19:24 2021 -0400

    Adding new labels to object detection model

commit a10ac3c
Merge: cd1c7af 3a2a47a
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 20:02:23 2021 -0400

    Merge remote-tracking branch 'upstream/main' into main

commit cd1c7af
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 20:01:46 2021 -0400

    Adding sample files for navigation vision

commit daee499
Author: MahimanaGIT <[email protected]>
Date:   Tue Mar 23 19:58:16 2021 -0400

    Updating object detection model

commit e90caf9
Author: Eli Benevedes <[email protected]>
Date:   Tue Mar 23 16:38:31 2021 -0400

    Fixed PR comments

commit 1a28576
Author: Ashay Aswale <[email protected]>
Date:   Tue Mar 23 15:41:40 2021 -0400

    WIP: state machine and resource localiser

commit fff20e7
Author: WaliaRohan <[email protected]>
Date:   Tue Mar 23 15:08:14 2021 -0400

    Added test file

commit 40a9944
Author: chintujaguar <[email protected]>
Date:   Tue Mar 23 14:55:31 2021 -0400

    Added comments to hauler nodes and creatated excavator nodes

commit 495056d
Author: Eli Benevedes <[email protected]>
Date:   Mon Mar 22 21:32:20 2021 -0400

    Remove extra waypoint in navigation server planner function call

commit 9dc4a1f
Merge: c062e5f 3a2a47a
Author: Eli Benevedes <[email protected]>
Date:   Mon Mar 22 21:26:27 2021 -0400

    Merged upstream with local repo

commit c062e5f
Author: Eli Benevedes <[email protected]>
Date:   Mon Mar 22 21:21:11 2021 -0400

    Nav code in a functional state for demo

commit 6327484
Author: Eli Benevedes <[email protected]>
Date:   Mon Mar 22 18:22:39 2021 -0400

    Lots of work done. Trajectory following now seems to work (roslaunch operations nav_test.launch, publish anything to cmd_vel). Next steps: Cleanup, refactor, comment, fully implement manual driving, then PR

commit 04506d1
Author: Eli Benevedes <[email protected]>
Date:   Mon Mar 22 00:24:47 2021 -0400

    Builds. Next step: test the navigation actionlib server.

commit fd3bfd9
Author: Eli Benevedes <[email protected]>
Date:   Sun Mar 21 14:42:59 2021 -0400

    Mid testing, tf2 is not working

commit 22085bf
Author: chintujaguar <[email protected]>
Date:   Fri Mar 19 18:34:51 2021 -0400

    Hauler bin position action client and server

commit 671cb2a
Merge: 084b005 911a72b
Author: Eli Benevedes <[email protected]>
Date:   Fri Mar 19 18:32:34 2021 -0400

    Merged + testing now running, but are failing (too many sig figs on output)

commit 911a72b
Merge: e34b74a c97380b
Author: Eli Benevedes <[email protected]>
Date:   Fri Mar 19 17:09:38 2021 -0400

    Merge pull request #1 from denniscr98/main

    Merging for further Gtesting and debugging

commit 084b005
Author: Eli Benevedes <[email protected]>
Date:   Fri Mar 19 17:09:23 2021 -0400

    More work done on navigation system. About to merge in test code.

commit c97380b
Author: Dennis Chavez Romero <[email protected]>
Date:   Thu Mar 11 17:00:52 2021 +0000

    unfinished testing for gtest.

commit bbdc326
Author: Dennis Chavez Romero <[email protected]>
Date:   Thu Mar 11 16:24:59 2021 +0000

    Began working on gtest for Nav algo.

commit f80e035
Merge: 683b477 e34b74a
Author: Dennis Chavez <[email protected]>
Date:   Thu Mar 11 10:24:35 2021 -0500

    Merge pull request #2 from Ronoman/navigation

    Update Navigation branch

commit 683b477
Merge: 04fefc7 39b65c7
Author: Dennis Chavez <[email protected]>
Date:   Thu Mar 11 10:18:58 2021 -0500

    Merge pull request #1 from Ronoman/main

    Updating fork

commit e34b74a
Author: Eli Benevedes <[email protected]>
Date:   Sun Mar 7 15:44:54 2021 -0500

    Updated tests to pull a function from another package. Began implementing a simple point-and-turn navigation solution.

commit c762d4d
Author: Eli Benevedes <[email protected]>
Date:   Fri Mar 5 15:11:02 2021 -0500

    Started updating actionlib server. Currently not working.

commit 4e34937
Author: Eli Benevedes <[email protected]>
Date:   Thu Mar 4 17:00:45 2021 -0500

    Added trajectory with velocities message, wrote test node to intake and process
  • Loading branch information
@AshayAswale
AshayAswale committed Mar 31, 2021
1 parent 4822550 commit f635c38
Show file tree
Hide file tree
Showing 38 changed files with 1,638 additions and 103 deletions.
6 changes: 0 additions & 6 deletions maploc/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,12 +189,6 @@ target_link_libraries(pr_dataset
${catkin_LIBRARIES}
)

# add_executable(pr_localization src/pr_localization.cpp)
# add_dependencies(pr_localization ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# target_link_libraries(pr_localization
# ${catkin_LIBRARIES}
# )

#############
## Install ##
#############
Expand Down
41 changes: 27 additions & 14 deletions maploc/src/publish_cheat_odom.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,15 @@ NASA SPACE ROBOTICS CHALLENGE

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Header.h>
#include <gazebo_msgs/GetModelState.h>
#include <tf/transform_broadcaster.h>
#include <utils/common_names.h>

#define UPDATE_HZ 200

std_msgs::Header last_header;

/**
* @brief This node is used to publish the ground truth position of the model given in the argument.
* It is just for testing and debugging should not be used in the finals.
Expand Down Expand Up @@ -57,24 +60,34 @@ int main(int argc, char **argv)

ros::Rate update_rate(UPDATE_HZ);

last_header.stamp = ros::Time(0);

while (ros::ok())
{
if (client.call(req))
{
transform.setOrigin(tf::Vector3(req.response.pose.position.x, req.response.pose.position.y, req.response.pose.position.z));
tf::quaternionMsgToTF(req.response.pose.orientation, quat);
transform.setRotation(quat);
transform.frame_id_ = COMMON_NAMES::MAP;
transform.child_frame_id_ = model_name + "_" + COMMON_NAMES::ROBOT_BASE;
transform.stamp_ = req.response.header.stamp;

br.sendTransform(transform);

odom_msg.pose.pose = req.response.pose;
odom_msg.twist.twist = req.response.twist;
odom_msg.header = req.response.header;
odom_msg.header.frame_id = COMMON_NAMES::MAP;
odom_pub.publish(odom_msg);
if(last_header.stamp != req.response.header.stamp) {
transform.setOrigin(tf::Vector3(req.response.pose.position.x, req.response.pose.position.y, req.response.pose.position.z));
tf::quaternionMsgToTF(req.response.pose.orientation, quat);
transform.setRotation(quat);
transform.frame_id_ = COMMON_NAMES::MAP;
transform.child_frame_id_ = model_name + "_" + COMMON_NAMES::ROBOT_BASE;
transform.stamp_ = req.response.header.stamp;

br.sendTransform(transform);

odom_msg.pose.pose = req.response.pose;
odom_msg.twist.twist = req.response.twist;
odom_msg.header = req.response.header;
odom_msg.header.frame_id = COMMON_NAMES::MAP;
odom_pub.publish(odom_msg);

last_header.stamp = req.response.header.stamp;
}
else
{
printf("Discarding duplicate transform\n");
}
}
else
{
Expand Down
67 changes: 58 additions & 9 deletions operations/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,18 +10,19 @@ project(operations)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
srcp2_msgs
actionlib_msgs
genmsg
message_generation
actionlib
std_msgs
utils
geometry_msgs
perception
gazebo_msgs
srcp2_msgs
tf2
tf2_ros
perception
)

## System dependencies are found with CMake's conventions
Expand Down Expand Up @@ -75,6 +76,9 @@ add_message_files(
add_action_files(DIRECTORY action
FILES
Navigation.action
ResourceLocaliser.action
Hauler.action
Excavator.action
)

generate_messages(
Expand Down Expand Up @@ -118,7 +122,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES operations
CATKIN_DEPENDS roscpp rospy actionlib_msgs std_msgs message_runtime utils geometry_msgs srcp2_msgs
CATKIN_DEPENDS roscpp rospy actionlib_msgs std_msgs message_runtime utils geometry_msgs srcp2_msgs perception
# DEPENDS system_lib
)

Expand All @@ -133,13 +137,10 @@ include_directories(
${catkin_INCLUDE_DIRS}
)

# add_library(${PROJECT_NAME}
# src/navigation_algorithm.cpp
# )

# Declare a C++ library
add_library(${PROJECT_NAME}
src/navigation_algorithm.cpp
src/scout_state_machine.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

Expand Down Expand Up @@ -172,18 +173,46 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})

# Drives both robots forwards in task 2. Example node
add_executable(navigation_actionlib_server src/navigation_actionlib_server.cpp)
add_executable(hauler_actionlib_server src/HaulerServerNew.cpp)
add_executable(hauler_actionlib_client src/HaulerClient.cpp)
add_executable(excavator_actionlib_client src/ExcavatorClient.cpp)
add_executable(excavator_actionlib_server src/ExcavatorServer.cpp)

add_dependencies(navigation_actionlib_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(excavator_actionlib_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(excavator_actionlib_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(navigation_actionlib_server ${PROJECT_NAME}
${catkin_LIBRARIES}
)

target_link_libraries(excavator_actionlib_server ${PROJECT_NAME}
${catkin_LIBRARIES}
)
target_link_libraries(excavator_actionlib_client ${PROJECT_NAME}
${catkin_LIBRARIES}
)
add_dependencies(hauler_actionlib_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(hauler_actionlib_server ${PROJECT_NAME}
${catkin_LIBRARIES}
)
add_dependencies(hauler_actionlib_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(hauler_actionlib_client ${PROJECT_NAME}
${catkin_LIBRARIES}
)
# Drives both robots forwards in task 2. Example node
add_executable(navigation_client src/navigation_client.cpp)
add_dependencies(navigation_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(navigation_client
${catkin_LIBRARIES}
)

# Drives both robots forwards in task 2. Example node
add_executable(resource_localiser src/resource_localiser.cpp)
add_dependencies(resource_localiser ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(resource_localiser
${catkin_LIBRARIES}
)

add_executable(trajectory_processor src/trajectory_processor.cpp)
add_dependencies(trajectory_processor ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(trajectory_processor
Expand All @@ -196,6 +225,26 @@ target_link_libraries(wheel_speed_processing
${catkin_LIBRARIES}
)

add_executable(resource_localiser_tester src/resource_localiser_tester.cpp)
add_dependencies(resource_localiser_tester ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(resource_localiser_tester
${catkin_LIBRARIES}
)

add_executable(navigation_vision src/navigation_vision.cpp)
add_dependencies(navigation_vision ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(navigation_vision
${catkin_LIBRARIES}
)

add_executable(start_scout_sm src/start_scout_sm.cpp)
add_dependencies(start_scout_sm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(start_scout_sm
${catkin_LIBRARIES} ${PROJECT_NAME}
)

# target_link_libraries(start_scout_sm navigation_algorithm)

#############
## Install ##
#############
Expand All @@ -207,7 +256,7 @@ target_link_libraries(wheel_speed_processing
## in contrast to setup.py, you can choose the destination

# All the cpp executable nodes should be listed here as well
install(TARGETS navigation_actionlib_server navigation_client trajectory_processor
install(TARGETS navigation_actionlib_server navigation_client trajectory_processor start_scout_sm
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
Expand Down Expand Up @@ -254,4 +303,4 @@ install(TARGETS navigation_actionlib_server navigation_client trajectory_process
# catkin_add_nosetests(test)

catkin_add_gtest(${PROJECT_NAME}-test test/algorithm_tests.cpp)
target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${PROJECT_NAME})
target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${PROJECT_NAME})
11 changes: 11 additions & 0 deletions operations/action/Excavator.action
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
# Define the goal
int8 task # The task variable to choose between digging or unloading
geometry_msgs/Point target # target point for shoulder yaw orientation
---
# Define the result
bool result_mass # Unused

---
# Define a feedback message
bool volatile_mass # Flag to check whether volatile present in scoop
bool regolith_mass # Flag to check whether regolith present in scoop
8 changes: 8 additions & 0 deletions operations/action/Hauler.action
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
# Define the goal
float64 desired_angle # The angle to empty the bin
---
# Define the result
float64 final_angle #
---
# Define a feedback message
float64 current_angle # Not using in current implementation due to lack of a feedback topic
6 changes: 2 additions & 4 deletions operations/action/Navigation.action
View file
Original file line number Diff line number Diff line change
@@ -1,11 +1,9 @@
# Define the goal
float32 forward_velocity # For manual driving
float32 angular_velocity # For manual driving

geometry_msgs/PointStamped point # Will not contain orientation information
float32 angular_velocity # For manual driving. Positive is counterclockwise, negative is clockwise

geometry_msgs/PoseStamped pose # Will contain orientation information
bool manual_driving # Whether or not this is a manual drive
uint32 drive_mode # Whether or not this is a manual drive. Corresponds to COMMON_NAMES::NAV_TYPE
---
# Define the result
# See COMMON_NAMES::NAV_RESULT for possible values
Expand Down
8 changes: 8 additions & 0 deletions operations/action/ResourceLocaliser.action
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
# Define the goal

---
# Define the result
uint32 resource_final_distance
---
# Define a feedback message
float32 percent_complete
6 changes: 6 additions & 0 deletions operations/include/operations/navigation_algorithm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,10 @@
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <utils/common_names.h>

using namespace COMMON_NAMES;

class NavigationAlgo
{
private:
Expand Down Expand Up @@ -165,6 +169,8 @@ class NavigationAlgo
*/
static double changeInPosition(const geometry_msgs::PoseStamped& current_robot_pose, const geometry_msgs::PoseStamped& target_robot_pose);

static double changeInOrientation(const geometry_msgs::PoseStamped& desired_pose, const std::string& robot_name, const tf2_ros::Buffer& tf_buffer);

};

#endif
95 changes: 95 additions & 0 deletions operations/include/operations/scout_state_machine.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
#ifndef SCOUT_STATE_MACHINE_H
#define SCOUT_STATE_MACHINE_H

#include <ros/ros.h>
#include <iostream>
#include <operations/NavigationAction.h>
#include <operations/ResourceLocaliserAction.h>
#include <actionlib/client/simple_action_client.h>
#include <srcp2_msgs/VolSensorMsg.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Empty.h>
#include <utils/common_names.h>
#include <mutex>

using namespace COMMON_NAMES;

enum LOCATOR_STATES
{
INIT = 0, // At the start of each session or after recharging,
// we would like to be at a certain location. Hence this state to get there
SEARCH , // SEARCH ALGORITHM
LOCATE, // Rotate and drive until distance minimises
FOUND, // Stop at the location for leading Excavator to the location
MOVE_OUT, // Make way for Excavator
RECHARGE, // Find and go to Recharge Station
WAIT_FOR_STATE_UPDATE // Do nothing
};


class ScoutStateMachine
{

private:
ros::NodeHandle nh_;

LOCATOR_STATES robot_state_ = LOCATOR_STATES::INIT;
std::string robot_name_;
bool state_machine_continue_ = true;

typedef actionlib::SimpleActionClient<operations::NavigationAction> NavigationClient_;
NavigationClient_* navigation_client_;
operations::NavigationGoal navigation_action_goal_;

typedef actionlib::SimpleActionClient<operations::ResourceLocaliserAction> ResourceLocaliserClient_;
ResourceLocaliserClient_* resource_localiser_client_;
operations::ResourceLocaliserGoal resource_localiser_goal_;

// Listens to the volatile sensor
ros::Subscriber volatile_sub_;

// Publishes the exact location of a localised volatile
ros::Publisher volatile_pub_;

// Listens for excavators indicating they are ready to pick up a volatile.
ros::Subscriber excavator_ready_sub_;

// Listens for robot odometry. Used to publish volatile location in FOUND
ros::Subscriber robot_odom_sub_;

void processVolatileMessage(const srcp2_msgs::VolSensorMsg::ConstPtr& vol_msg);
void processOdomMessage(const nav_msgs::Odometry::ConstPtr& odom_msg);
void processExcavatorMessage(const std_msgs::Empty::ConstPtr& excavator_msg);

// Whether or not we are on the first iteration of a state.
// Reset to true on state transition, set to false after first iteration.
bool first_iter_ = true;

// Whether a volatile is currently detected. Reset to false at the beginning of SEARCH
bool vol_detected_ = false;

// Whether an excavator has arrived and is ready to pick up a volatile. Reset to false at the beginning of FOUND
bool excavator_ready_ = false;

// Most recent robot pose from odometry
geometry_msgs::PoseStamped robot_pose_;

// Mutex-protected way to get robot_pose_
geometry_msgs::PoseStamped getRobotPose();

// Mutexex to protect flags set in subscribers and read in the SM
std::mutex vol_flag_mutex_;
std::mutex robot_pose_mutex_;
std::mutex excavator_ready_mutex_;

public:
ScoutStateMachine(ros::NodeHandle nh, const std::string& robot_name);

~ScoutStateMachine();

void startStateMachine();
void stopStateMachine(); // Do we need it though?
};

#endif // SCOUT_STATE_MACHINE_H
Loading

0 comments on commit f635c38

Please sign in to comment.