diff --git a/maploc/CMakeLists.txt b/maploc/CMakeLists.txt index a21e2ed1..a4ec3345 100644 --- a/maploc/CMakeLists.txt +++ b/maploc/CMakeLists.txt @@ -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 ## ############# diff --git a/maploc/src/publish_cheat_odom.cpp b/maploc/src/publish_cheat_odom.cpp index bc6c5480..08f64d9a 100644 --- a/maploc/src/publish_cheat_odom.cpp +++ b/maploc/src/publish_cheat_odom.cpp @@ -9,12 +9,15 @@ NASA SPACE ROBOTICS CHALLENGE #include #include +#include #include #include #include #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. @@ -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 { diff --git a/operations/CMakeLists.txt b/operations/CMakeLists.txt index 797892aa..ece1dcf1 100644 --- a/operations/CMakeLists.txt +++ b/operations/CMakeLists.txt @@ -10,7 +10,6 @@ project(operations) find_package(catkin REQUIRED COMPONENTS roscpp rospy - srcp2_msgs actionlib_msgs genmsg message_generation @@ -18,10 +17,12 @@ find_package(catkin REQUIRED COMPONENTS std_msgs utils geometry_msgs + perception gazebo_msgs srcp2_msgs tf2 tf2_ros + perception ) ## System dependencies are found with CMake's conventions @@ -75,6 +76,9 @@ add_message_files( add_action_files(DIRECTORY action FILES Navigation.action + ResourceLocaliser.action + Hauler.action + Excavator.action ) generate_messages( @@ -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 ) @@ -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}) @@ -172,11 +173,32 @@ 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}) @@ -184,6 +206,13 @@ 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 @@ -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 ## ############# @@ -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}) @@ -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}) \ No newline at end of file +target_link_libraries(${PROJECT_NAME}-test ${catkin_LIBRARIES} ${PROJECT_NAME}) diff --git a/operations/action/Excavator.action b/operations/action/Excavator.action new file mode 100644 index 00000000..153c138a --- /dev/null +++ b/operations/action/Excavator.action @@ -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 \ No newline at end of file diff --git a/operations/action/Hauler.action b/operations/action/Hauler.action new file mode 100644 index 00000000..22c1b557 --- /dev/null +++ b/operations/action/Hauler.action @@ -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 \ No newline at end of file diff --git a/operations/action/Navigation.action b/operations/action/Navigation.action index 40b802d3..51945f98 100644 --- a/operations/action/Navigation.action +++ b/operations/action/Navigation.action @@ -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 diff --git a/operations/action/ResourceLocaliser.action b/operations/action/ResourceLocaliser.action new file mode 100644 index 00000000..a0b8ff50 --- /dev/null +++ b/operations/action/ResourceLocaliser.action @@ -0,0 +1,8 @@ +# Define the goal + +--- +# Define the result +uint32 resource_final_distance +--- +# Define a feedback message +float32 percent_complete \ No newline at end of file diff --git a/operations/include/operations/navigation_algorithm.h b/operations/include/operations/navigation_algorithm.h index 2024ca28..43e7eed4 100644 --- a/operations/include/operations/navigation_algorithm.h +++ b/operations/include/operations/navigation_algorithm.h @@ -10,6 +10,10 @@ #include #include +#include + +using namespace COMMON_NAMES; + class NavigationAlgo { private: @@ -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 \ No newline at end of file diff --git a/operations/include/operations/scout_state_machine.h b/operations/include/operations/scout_state_machine.h new file mode 100644 index 00000000..175e7d0b --- /dev/null +++ b/operations/include/operations/scout_state_machine.h @@ -0,0 +1,95 @@ +#ifndef SCOUT_STATE_MACHINE_H +#define SCOUT_STATE_MACHINE_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 NavigationClient_; + NavigationClient_* navigation_client_; + operations::NavigationGoal navigation_action_goal_; + + typedef actionlib::SimpleActionClient 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 \ No newline at end of file diff --git a/operations/launch/nav_test.launch b/operations/launch/nav_test.launch index 62496396..bf936c87 100644 --- a/operations/launch/nav_test.launch +++ b/operations/launch/nav_test.launch @@ -1,13 +1,10 @@ - + + - - - - \ No newline at end of file diff --git a/operations/launch/nav_vision_test.launch b/operations/launch/nav_vision_test.launch new file mode 100644 index 00000000..3d8201de --- /dev/null +++ b/operations/launch/nav_vision_test.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/operations/launch/navigation.launch b/operations/launch/navigation.launch new file mode 100644 index 00000000..391bf4cd --- /dev/null +++ b/operations/launch/navigation.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/operations/launch/navigation_vision_test.launch b/operations/launch/navigation_vision_test.launch new file mode 100644 index 00000000..fef50d55 --- /dev/null +++ b/operations/launch/navigation_vision_test.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/operations/launch/start_scout.launch b/operations/launch/start_scout.launch new file mode 100644 index 00000000..297dfee9 --- /dev/null +++ b/operations/launch/start_scout.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/operations/package.xml b/operations/package.xml index addbdf70..a50ac9fe 100644 --- a/operations/package.xml +++ b/operations/package.xml @@ -61,8 +61,11 @@ message_generation message_runtime utils + perception + geometry_msgs srcp2_msgs + perception roscpp rospy @@ -73,7 +76,9 @@ message_runtime utils geometry_msgs + perception srcp2_msgs + perception roscpp rospy @@ -84,7 +89,9 @@ message_runtime utils geometry_msgs + perception srcp2_msgs + perception rosunit diff --git a/operations/src/ExcavatorClient.cpp b/operations/src/ExcavatorClient.cpp new file mode 100644 index 00000000..f59a0bb4 --- /dev/null +++ b/operations/src/ExcavatorClient.cpp @@ -0,0 +1,64 @@ +#include +#include +#include +#include + +using namespace COMMON_NAMES; + +/** + * @brief The task assigned numbers + * + */ +enum Tasks{ + START_DIGGING = 1, // This starts the digging condition + START_UNLOADING = 2, // This starts the unloading condition + SLEEP_DURATION = 5 // The sleep duration +}; + +typedef actionlib::SimpleActionClient Client; + +/** + * @brief The main method for excavator client + * + * @param argc The number of arguments passed + * @param argv The array of arguments passed + * @return int + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "excavator_client"); + Client client(EXCAVATOR_ACTIONLIB, true); + client.waitForServer(); + + operations::ExcavatorGoal goal; + goal.task = START_DIGGING; // START_DIGGING = 1 + goal.target.x = 0.7; // set of target digging values to the left of the excavator + goal.target.y = 2; + goal.target.z = 0; + client.sendGoal(goal); + + std::string message1(client.getState().toString().c_str()); + ROS_INFO_STREAM("Current State: " + message1); + client.waitForResult(ros::Duration(SLEEP_DURATION)); + if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + ROS_INFO_STREAM("Yay! The scoop is now full"); + + ros::Duration(SLEEP_DURATION).sleep(); // Delay between digging and unloading tasks + + goal.task = START_UNLOADING; // START_UNLOADING = 2 + goal.target.x = 0.7; // set of target dumping values to the right of the excavator + goal.target.y = -2; + goal.target.z = 0; + client.sendGoal(goal); + + std::string message2(client.getState().toString().c_str()); + ROS_INFO_STREAM("Current State: " + message2); + client.waitForResult(ros::Duration(SLEEP_DURATION)); + if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + ROS_INFO_STREAM("Yay! The scoop is now empty"); + + std::string message3(client.getState().toString().c_str()); + ROS_INFO_STREAM("Current State: " + message3); + + return 0; +} \ No newline at end of file diff --git a/operations/src/ExcavatorServer.cpp b/operations/src/ExcavatorServer.cpp new file mode 100644 index 00000000..ede2b26a --- /dev/null +++ b/operations/src/ExcavatorServer.cpp @@ -0,0 +1,192 @@ +#include +#include +#include +#include "ros/ros.h" +#include +#include +#include // To get target point in order to orient shoulder joint +#include // used in findShoulderAngle() for atan2() + +typedef actionlib::SimpleActionServer Server; +ros::Publisher excavator_shoulder_yaw_publisher_; +ros::Publisher excavator_shoulder_pitch_publisher_; +ros::Publisher excavator_elbow_pitch_publisher_; +ros::Publisher excavator_wrist_pitch_publisher_; +using namespace COMMON_NAMES; + +// The global variables to save the last values passed to the joints +float curr_sh_yaw = 0; +float curr_sh_pitch = 0; +float curr_elb_pitch = 0; +float curr_wrt_pitch = 0; + +// The task numbers and sleep duration +enum Tasks{ + START_DIGGING = 1, // This starts the digging condition + START_UNLOADING = 2, // This starts the unloading condition + SLEEP_DURATION = 2 // The sleep duration +}; + +/** + * @brief Initializing the publisher here + * + * @param nh nodeHandle + * @param robot_name Passed in the terminal/launch file to target a particular rover + */ +void initExcavatorPublisher(ros::NodeHandle &nh, const std::string &robot_name) +{ + excavator_shoulder_yaw_publisher_ = nh.advertise(robot_name + SET_SHOULDER_YAW_POSITION, 1000); + excavator_shoulder_pitch_publisher_ = nh.advertise(robot_name + SET_SHOULDER_PITCH_POSITION, 1000); + excavator_elbow_pitch_publisher_ = nh.advertise(robot_name + SET_ELBOW_PITCH_POSITION, 1000); + excavator_wrist_pitch_publisher_ = nh.advertise(robot_name + SET_WRIST_PITCH_POSITION, 1000); +} + +/** + * @brief Calculates the orientation of the shoulder joint based on the passed target point + * + * @param target the x, y coordinates of the volatiles in the body frame of the excavator + * @param shoulder the x, y coordinates of the shoulder joint in the body frame of excavator + * @return atan2((target.y - shoulder.y), (target.x - shoulder.x)) is the required yaw angle of the shoulder joint + */ +float findShoulderAngle(const geometry_msgs::Point &target, const geometry_msgs::Point &shoulder) +{ + return atan2((target.y - shoulder.y), (target.x - shoulder.x)); +} + +/** + * @brief This function publishes the joint angles to the publishers + * + * @param values array of joint angles (ordered) + */ + +/** + * @brief This function publishes the joint angles to the publishers + * + * @param shoulder_yaw the desired shoulder yaw joint value + * @param shoulder_pitch the desired shoulder pitch joint value + * @param elbow_pitch the desired elbow pitch joint value + * @param wrist_pitch the desired wrist pitch joint value + * @param steps the number of simulation steps, higher value corresponds to slower movement + * @param dig flag for dig or dump task to adjust the speed + */ +void publishAngles(float shoulder_yaw, float shoulder_pitch, float elbow_pitch, float wrist_pitch, int steps, bool dig) +{ + std_msgs::Float64 shoulder_yaw_msg; + std_msgs::Float64 shoulder_pitch_msg; + std_msgs::Float64 elbow_pitch_msg; + std_msgs::Float64 wrist_pitch_msg; + + for(int i=0; itask == START_DIGGING) // START_DIGGING = 1 + { + publishExcavatorMessage(START_DIGGING, goal->target, shoulder); + ros::Duration(SLEEP_DURATION).sleep(); + // action_server->working(); // might use for feedback + action_server->setSucceeded(); + } + else if (goal->task == START_UNLOADING) // START_UNLOADING = 2 + { + publishExcavatorMessage(START_UNLOADING, goal->target, shoulder); + ros::Duration(SLEEP_DURATION).sleep(); + action_server->setSucceeded(); + } +} + +/** + * @brief The main method for excavator server + * + * @param argc The number of arguments passed + * @param argv The array of arguments passed + * @return int + */ +int main(int argc, char** argv) +{ + ROS_INFO_STREAM(std::to_string(argc) + "\n"); + // Check if the node is being run through roslauch, and have one parameter of RobotName_Number + if (argc != 2) + { + // Displaying an error message for correct usage of the script, and returning error. + ROS_ERROR_STREAM("This Node must be launched via 'roslaunch' and needs an argument as "); + return -1; + } + else + { + std::string robot_name = (std::string)argv[1]; + ROS_INFO_STREAM(robot_name + "\n"); + std::string node_name = robot_name + "_excavator_action_server"; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + initExcavatorPublisher(nh, robot_name); + Server server(nh, EXCAVATOR_ACTIONLIB, boost::bind(&execute, _1, &server), false); + server.start(); + ros::spin(); + + return 0; + } +} \ No newline at end of file diff --git a/operations/src/HaulerClient.cpp b/operations/src/HaulerClient.cpp new file mode 100644 index 00000000..192d23d4 --- /dev/null +++ b/operations/src/HaulerClient.cpp @@ -0,0 +1,26 @@ +#include +#include +#include +#include + +using namespace COMMON_NAMES; + +typedef actionlib::SimpleActionClient Client; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "hauler_bin_client"); + Client client(HAULER_ACTIONLIB, true); + client.waitForServer(); + operations::HaulerGoal goal; + goal.desired_angle = 1; + client.sendGoal(goal); + std::string message1(client.getState().toString().c_str()); + ROS_INFO_STREAM("Current State: " + message1); + client.waitForResult(ros::Duration(5.0)); + if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + ROS_INFO_STREAM("Yay! The hauler is now empty"); + std::string message2(client.getState().toString().c_str()); + ROS_INFO_STREAM("Current State: " + message2); + return 0; +} \ No newline at end of file diff --git a/operations/src/HaulerServerNew.cpp b/operations/src/HaulerServerNew.cpp new file mode 100644 index 00000000..0b93c5ec --- /dev/null +++ b/operations/src/HaulerServerNew.cpp @@ -0,0 +1,90 @@ +#include // Note: "Action" is appended +#include +#include +#include "ros/ros.h" +#include +#include + +typedef actionlib::SimpleActionServer Server; +ros::Publisher hauler_bin_publisher_; +using namespace COMMON_NAMES; + +float BIN_RESET = 0.0; // This returns the bin to initial position +float BIN_EMPTY = 3.0; // This is the angle to empty the bin +float SLEEP_DURATION = 5.0; // The sleep duration + +/** + * @brief Initializing the publisher here + * + * @param nh nodeHandle + * @param robot_name Passed in the terminal/launch file to target a particular rover + */ +void initHaulerBinPublisher(ros::NodeHandle &nh, const std::string &robot_name) +{ + hauler_bin_publisher_ = nh.advertise(robot_name + SET_BIN_POSITION, 1000); +} + +/** + * @brief publishes the bin angle to the rostopic small_hauler_1/bin/command/position + * + * @param data the bin angle + */ +void publishHaulerBinMessage(float data) +{ + std_msgs::Float64 pub_data; + pub_data.data = data; + hauler_bin_publisher_.publish(pub_data); +} + +/** + * @brief This is where the action is executed if bin_angle is satisfied + * + * @param goal The desired bin angle + * @param action_server The server object for hauler action + */ +void execute(const operations::HaulerGoalConstPtr& goal, Server* action_server) +{ + if (goal->desired_angle > BIN_RESET) // BIN_RESET = 0 + { + publishHaulerBinMessage(BIN_EMPTY); + // action_server->working(); // might use for feedback + ros::Duration(SLEEP_DURATION).sleep(); + publishHaulerBinMessage(BIN_RESET); + action_server->setSucceeded(); + } +} + +/** + * @brief The main method for hauler server + * + * @param argc The number of arguments passed + * @param argv The array of arguments passed + * @return int + */ +int main(int argc, char** argv) +{ + ROS_INFO_STREAM(std::to_string(argc) + "\n"); + // Check if the node is being run through roslauch, and have one parameter of RobotName_Number + if (argc != 2) + { + // Displaying an error message for correct usage of the script, and returning error. + ROS_ERROR_STREAM("This Node must be launched via 'roslaunch' and needs an argument as "); + return -1; + } + else + { + std::string robot_name = (std::string)argv[1]; + ROS_INFO_STREAM(robot_name + "\n"); + std::string node_name = robot_name + "_hauler_action_server"; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + // Initialise the publishers for steering and wheel velocites + initHaulerBinPublisher(nh, robot_name); + // Action server + Server server(nh, HAULER_ACTIONLIB, boost::bind(&execute, _1, &server), false); + server.start(); + ros::spin(); + + return 0; + } +} \ No newline at end of file diff --git a/operations/src/navigation_actionlib_server.cpp b/operations/src/navigation_actionlib_server.cpp index 6de1e705..dd7983ff 100644 --- a/operations/src/navigation_actionlib_server.cpp +++ b/operations/src/navigation_actionlib_server.cpp @@ -1,7 +1,5 @@ #include -#include - #include #include #include @@ -25,7 +23,7 @@ using namespace COMMON_NAMES; // Base speed for rotating and driving the robot. TODO: PID controller does not properly match this value -#define BASE_SPEED 0.25 +#define BASE_SPEED 0.6 // Tolerances for drives and turns #define DIST_EPSILON 0.05 @@ -242,7 +240,8 @@ operations::TrajectoryWithVelocities* sendGoalToPlanner(const operations::Naviga return traj; } -void brakeRobot(double brake_force){ +void brakeRobot(double brake_force) +{ srcp2_msgs::BrakeRoverSrv srv; srv.request.brake_force = brake_force; brake_client_.call(srv); @@ -260,13 +259,13 @@ bool rotateRobot(const geometry_msgs::PoseStamped& target_robot_pose) geometry_msgs::PoseStamped starting_pose = *getRobotPose(); double delta_heading = NavigationAlgo::changeInHeading(starting_pose, target_robot_pose, robot_name, buffer); - printf("Turning %frad\n", delta_heading); - - if (delta_heading == 0) + + if (abs(delta_heading) <= ANGLE_EPSILON) { return true; } + printf("Turning %frad\n", delta_heading); steerRobot(wheel_angles); // While we have not turned the desired amount @@ -353,6 +352,8 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac //Forward goal to local planner, and save the returned trajectory operations::TrajectoryWithVelocities *trajectory = sendGoalToPlanner(goal); + geometry_msgs::PoseStamped final_pose = buffer.transform(goal->pose, MAP, ros::Duration(0.1)); + //Loop over trajectories for (int i = 0; i < trajectory->waypoints.size(); i++) { @@ -360,7 +361,7 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac { ROS_ERROR_STREAM("Overridden by manual driving! Exiting.\n"); operations::NavigationResult res; - res.result = COMMON_NAMES::NAV_RESULT::INTERRUPTED; + res.result = NAV_RESULT::INTERRUPTED; action_server->setSucceeded(res); // Cleanup trajectory @@ -377,7 +378,7 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac current_waypoint.header.stamp = ros::Time(0); - current_waypoint = buffer.transform(current_waypoint, COMMON_NAMES::MAP, ros::Duration(0.1)); + current_waypoint = buffer.transform(current_waypoint, MAP, ros::Duration(0.1)); // Needed, otherwise we get extrapolation into the past current_waypoint.header.stamp = ros::Time(0); @@ -392,13 +393,13 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac if(manual_driving) { ROS_ERROR_STREAM("Overridden by manual driving! Exiting.\n"); - res.result = COMMON_NAMES::NAV_RESULT::INTERRUPTED; + res.result = NAV_RESULT::INTERRUPTED; } else { //AAAH ERROR ROS_ERROR_STREAM("Turn to waypoint " << i << " did not succeed. Exiting.\n"); - res.result = COMMON_NAMES::NAV_RESULT::FAILED; + res.result = NAV_RESULT::FAILED; } action_server->setSucceeded(res); @@ -424,7 +425,7 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac if(manual_driving) { ROS_ERROR_STREAM("Overridden by manual driving! Exiting.\n"); - res.result = COMMON_NAMES::NAV_RESULT::INTERRUPTED; + res.result = NAV_RESULT::INTERRUPTED; } else { //AAAH ERROR @@ -440,37 +441,45 @@ void automaticDriving(const operations::NavigationGoalConstPtr &goal, Server *ac } } - //TODO: Change condition to run if we got a pose instead of a point - if (false) - { - geometry_msgs::PoseStamped final_pose = goal->pose; + geometry_msgs::PoseStamped current_robot_pose = *getRobotPose(); - //Turn to heading - bool turned_successfully = rotateRobot(final_pose); + // The final pose is on top of the robot, we only care about orientation + final_pose.pose.position.x = current_robot_pose.pose.position.x; + final_pose.pose.position.y = current_robot_pose.pose.position.y; - if (!turned_successfully) - { - operations::NavigationResult res; + final_pose.header.stamp = ros::Time(0); - if(manual_driving) - { - ROS_ERROR_STREAM("Overridden by manual driving! Exiting.\n"); - res.result = NAV_RESULT::INTERRUPTED; - } - else - { - //AAAH ERROR - ROS_ERROR_STREAM("Final turn did not succeed. Exiting.\n"); - res.result = NAV_RESULT::FAILED; - - } - action_server->setSucceeded(res); + printf("Final rotate\n"); + + //Turn to heading + bool turned_successfully = rotateRobot(final_pose); + + if (!turned_successfully) + { + operations::NavigationResult res; + + if(manual_driving) + { + ROS_ERROR_STREAM("Overridden by manual driving! Exiting.\n"); + res.result = NAV_RESULT::INTERRUPTED; + } + else + { + //AAAH ERROR + ROS_ERROR_STREAM("Final turn did not succeed. Exiting.\n"); + res.result = NAV_RESULT::FAILED; + } + action_server->setSucceeded(res); } // Cleanup trajectory delete trajectory; + printf("Finished automatic goal!\n"); + + brakeRobot(1000); + operations::NavigationResult res; res.result = NAV_RESULT::SUCCESS; action_server->setSucceeded(res); @@ -513,26 +522,78 @@ void angularDriving(const operations::NavigationGoalConstPtr &goal, Server *acti return; } +void spiralDriving(const operations::NavigationGoalConstPtr &goal, Server *action_server) +{ + printf("Spiral drive: Spiral Away!\n"); + + ros::Duration(5).sleep(); + + //TODO: actually make it spiral + + operations::NavigationResult res; + res.result = NAV_RESULT::SUCCESS; + action_server->setSucceeded(res); + return; +} + +void followDriving(const operations::NavigationGoalConstPtr &goal, Server *action_server) +{ + printf("Follow drive: Following!\n"); + + ros::Duration(5).sleep(); + + //TODO: actually make it follow the thing + + operations::NavigationResult res; + res.result = NAV_RESULT::SUCCESS; + action_server->setSucceeded(res); + return; +} + void execute(const operations::NavigationGoalConstPtr &goal, Server *action_server) { printf("Received NavigationGoal, dispatching\n"); - if(goal->manual_driving) - { - manual_driving = true; - if(goal->angular_velocity != 0) - { - angularDriving(goal, action_server); - } - else - { - linearDriving(goal, action_server); - } - } - else + switch(goal->drive_mode) { - manual_driving = false; - automaticDriving(goal, action_server); + case NAV_TYPE::MANUAL: + + manual_driving = true; + if(goal->angular_velocity != 0) + { + angularDriving(goal, action_server); + } + else + { + linearDriving(goal, action_server); + } + + break; + + case NAV_TYPE::GOAL: + + manual_driving = false; + automaticDriving(goal, action_server); + + break; + + case NAV_TYPE::SPIRAL: + + manual_driving = false; + spiralDriving(goal, action_server); + + break; + + case NAV_TYPE::FOLLOW: + + manual_driving = false; + followDriving(goal, action_server); + + break; + + default: + ROS_ERROR_STREAM(robot_name + " encountered an unknown driving mode!"); + break; } } diff --git a/operations/src/navigation_algorithm.cpp b/operations/src/navigation_algorithm.cpp index 783f5f5a..0f243c7c 100644 --- a/operations/src/navigation_algorithm.cpp +++ b/operations/src/navigation_algorithm.cpp @@ -184,11 +184,20 @@ double NavigationAlgo::changeInPosition(const geometry_msgs::PoseStamped& curren double NavigationAlgo::changeInHeading(const geometry_msgs::PoseStamped& current_robot_pose, const geometry_msgs::PoseStamped& current_waypoint, const std::string& robot_name, const tf2_ros::Buffer& tf_buffer) { + //printf("%f\n", chan) + + // Hack (kind of) for Ashay. If the two poses are within 15cm of each other, we assume that we want to match orientations, not turn to face a waypoint + // TODO: Make sure planner team doesn't give us two waypoints that is within this tolerance + if(changeInPosition(current_robot_pose, current_waypoint) < 0.15) + { + return changeInOrientation(current_waypoint, robot_name, tf_buffer); + } + // Get the next waypoint in the robot's frame geometry_msgs::PoseStamped waypoint_relative_to_robot; // Should probably wrap this in a try except for tf2::ExtrapolationException, we seem to extrapolate into the past sometimes - waypoint_relative_to_robot = tf_buffer.transform(current_waypoint, robot_name + "_small_chassis", ros::Duration(0.1)); + waypoint_relative_to_robot = tf_buffer.transform(current_waypoint, robot_name + ROBOT_CHASSIS, ros::Duration(0.1)); //Get the change in yaw between the two poses with atan2 double change_in_yaw = atan2(waypoint_relative_to_robot.pose.position.y, waypoint_relative_to_robot.pose.position.x); @@ -205,4 +214,10 @@ double NavigationAlgo::changeInHeading(const geometry_msgs::PoseStamped& current } return change_in_yaw; +} + +double NavigationAlgo::changeInOrientation(const geometry_msgs::PoseStamped& desired_pose, const std::string& robot_name, const tf2_ros::Buffer& tf_buffer) +{ + geometry_msgs::PoseStamped relative_to_robot = tf_buffer.transform(desired_pose, robot_name + ROBOT_CHASSIS, ros::Duration(0.1)); + return fromQuatToEuler(relative_to_robot)[2]; } \ No newline at end of file diff --git a/operations/src/navigation_client.cpp b/operations/src/navigation_client.cpp index 657ecff7..ef1f5183 100644 --- a/operations/src/navigation_client.cpp +++ b/operations/src/navigation_client.cpp @@ -28,7 +28,7 @@ void teleopCB(const geometry_msgs::Twist::ConstPtr& twist) operations::NavigationGoal goal; // Manual driving - goal.manual_driving = true; + goal.drive_mode = NAV_TYPE::MANUAL; // Diverting values from twist to navigation goal.forward_velocity = twist->linear.x; @@ -51,22 +51,28 @@ void navigationCB(const geometry_msgs::Point::ConstPtr& goal_point) //Simple waypoint 2 meters in front of the robot geometry_msgs::PoseStamped t1; t1.header.frame_id = robot_name + "_small_chassis"; + t1.header.stamp = ros::Time::now(); t1.pose.position.x = goal_point->x; t1.pose.position.y = goal_point->y; t1.pose.position.z = 0; - t1.pose.orientation.w = 1; + t1.pose.orientation.w = 0.707; t1.pose.orientation.x = 0; t1.pose.orientation.y = 0; - t1.pose.orientation.z = 0; + t1.pose.orientation.z = 0.707; goal.pose = t1; - goal.manual_driving = false; + goal.drive_mode = NAV_TYPE::GOAL; printf("Sending auto goal to actionlib server\n"); client->sendGoal(goal); - ros::Duration(0.1).sleep(); + // ros::Duration(0.1).sleep(); + // printf("Re-sending auto goal to actionlib server\n"); + // goal.manual_driving = false; + // goal.forward_velocity = 0; + // goal.angular_velocity = 0; + // client->sendGoal(goal); } int main(int argc, char** argv) diff --git a/operations/src/navigation_vision.cpp b/operations/src/navigation_vision.cpp new file mode 100644 index 00000000..af81df9d --- /dev/null +++ b/operations/src/navigation_vision.cpp @@ -0,0 +1,151 @@ +/* TODO: +- Tune the PID controller for turning +- Investigate rightward jerking at beginning of turn +- Convert into action library + - Input: Label + - Output: Progress, Done? +*/ + +#include // Note: "Action" is appended +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +// typedef for the Action Server +typedef actionlib::SimpleActionClient Client; +Client* client; + +using namespace COMMON_NAMES; + +std::string robot_name; +float sum_error_angle = 0.0; +float prev_error_angle = 0.0; +float sum_error_height = 0.0; +float prev_error_height = 0.0; + +void objects_callback(const perception::ObjectArray& objects) +{ + ROS_INFO("Working callback"); + + bool obj_detected; + + // Create a goal object and turn on manual driving + operations::NavigationGoal goal; + goal.drive_mode = NAV_TYPE::MANUAL; + + // Initialize location and size variables + float center_obj = -1; + float height_obj = -1; + float width = 640.0; + + // Find the desired object + for(int i = 0; i < objects.number_of_objects; i++) + { + if(objects.obj[i].label == "repairStation") { + // Store the object's center and height + center_obj = objects.obj[i].center.x; + height_obj = objects.obj[i].size_y; + } + } + + // Initialize error, P Control, and necessary thresholds + float proportional_angle = 0.001; + float integral_angle = 0.0000001; + float derivative_angle = 0.001; + float error_angle; + int error_angle_threshold = 40; + + float proportional_height = 0.02; + float integral_height = 0.0; + float derivative_height = 0.0; + float error_height; + int height_threshold = 300; + int error_height_threshold = 10; + + if(center_obj == -1) + { + obj_detected = false; + goal.angular_velocity = 0.3; + goal.forward_velocity = 0; + } + else + { + obj_detected = true; + error_angle = (width / 2.0) - center_obj; + error_height = height_threshold - height_obj; + + + if(abs(error_angle) < error_angle_threshold) + { + // If object is centered, stop turning. + goal.angular_velocity = 0; + + if(error_height < error_height_threshold) + { + // If the object is big enough, stop the robot + goal.forward_velocity = 0; + client->sendGoal(goal); + ros::shutdown(); + } + else + { + // Keep driving forward + sum_error_height += error_height; + goal.forward_velocity = error_height * proportional_height + sum_error_height * integral_height + (error_height - prev_error_height) * derivative_height; + } + } + else + { + // If object is not centered, turn. + sum_error_angle += error_angle; + goal.angular_velocity = error_angle * proportional_angle + sum_error_angle * integral_angle + (error_angle - prev_error_angle) * derivative_angle; + goal.forward_velocity = 0; + } + } + + prev_error_angle = error_angle; + prev_error_height = error_height; + + ROS_INFO_STREAM("ERROR Height: "<sendGoal(goal); + +} + +int main(int argc, char** argv) +{ + // Ensure the robot name is passed in + if (argc != 2 && argc != 4) + { + // Displaying an error message for correct usage of the script, and returning error. + ROS_ERROR_STREAM("Not enough arguments! Please pass in robot name with number."); + return -1; + } + else + { + // Robot Name from argument + robot_name = argv[1]; + std::string node_name = robot_name + "_navigation_action_client"; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + + // Subscribing to teleop topic + ros::Subscriber objects_sub = nh.subscribe(COMMON_NAMES::CAPRICORN_TOPIC + robot_name + COMMON_NAMES::OBJECT_DETECTION_OBJECTS_TOPIC, 1, &objects_callback); + // initialize client + client = new Client(NAVIGATION_ACTIONLIB, true); + printf("Waiting for server...\n"); + client->waitForServer(); + printf("Done waiting. Spinning\n"); + + ros::spin(); + return 0; + } +} \ No newline at end of file diff --git a/operations/src/resource_localiser.cpp b/operations/src/resource_localiser.cpp new file mode 100644 index 00000000..2d0bbb48 --- /dev/null +++ b/operations/src/resource_localiser.cpp @@ -0,0 +1,265 @@ +#include // Note: "Action" is appended +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + + +// typedef for the Action Server and Client +typedef actionlib::SimpleActionServer ResourceLocaliserServer; +typedef actionlib::SimpleActionClient NavigationClient_; +NavigationClient_* navigation_client_; + + +using namespace COMMON_NAMES; + + +double ROTATION_VELOCITY = 0.2; +double MAX_DETECT_DIST = 2.0; +double VOLATILE_DISTANCE_THRESHOLD = 0.02; +int FLIP_ROTATION_COUNT_MAX = 2; +bool near_volatile_ = false; +bool new_message_received = false; +double volatile_distance_; + +geometry_msgs::PoseStamped robot_pose_; + + +/** + * @brief enum for rotation direction + * When clockwise, send direction as it is + When Anticlockwise, flip the direction with -1 + * + */ +enum RotationDirection +{ + CLOCKWISE = 1, + COUNTERCLOCKWISE = -1 +}; + + +/** + * @brief Rotate the robot in the given direction + * + * @param rotate_direction direction of rotation + */ +void rotateRobot(const RotationDirection rotate_direction) +{ + operations::NavigationGoal goal; + + // Manual driving + goal.drive_mode = NAV_TYPE::MANUAL; + + goal.forward_velocity = 0; + goal.angular_velocity = rotate_direction * ROTATION_VELOCITY; + + navigation_client_->sendGoal(goal); + ros::Duration(0.5).sleep(); +} + +/** + * @brief Stop the robot + * + * + */ +void stopRobot() +{ + operations::NavigationGoal goal; + + // Manual driving + goal.drive_mode = NAV_TYPE::MANUAL; + + goal.forward_velocity = 0; + goal.angular_velocity = 0; + + navigation_client_->sendGoal(goal); + // ros::Duration(0.5).sleep(); +} + +/** + * @brief Rotate the robot to the absolute yaw + * + * @param orientation yaw of the robot + * ref frame is 'map' + */ +void navigateRobot(const geometry_msgs::Pose target_pose) +{ + operations::NavigationGoal goal; + + // Auto driving + goal.drive_mode = NAV_TYPE::GOAL; + + goal.pose.header.frame_id = MAP; + goal.pose.pose = target_pose; + + navigation_client_->sendGoal(goal); + // ros::Duration(0.1).sleep(); +} + +/** + * @brief Get the Best Pose object + * Currently only adjusts the orientation + * Should be generalised for position as well + * + * @return geometry_msgs::Pose Pose which minimises the volatile distance + */ +geometry_msgs::Pose getBestPose() +{ + RotationDirection rotate_direction = CLOCKWISE; + bool rotate_robot = true; + int flip_rotation_count = 0; + + double last_volatile_distance = MAX_DETECT_DIST + 1; // To make sure any detected + // distance is less than this + double best_volatile_distance = last_volatile_distance; + + geometry_msgs::Pose current_robot_pose = robot_pose_.pose; + geometry_msgs::Pose best_robot_pose = current_robot_pose; + + // Start rotating the robot to minimise distance + rotateRobot(rotate_direction); + + while (rotate_robot && ros::ok()) + { + if(new_message_received) + { + new_message_received = false; + current_robot_pose = (robot_pose_.pose); + + // If the distance is decreasing + if ((last_volatile_distance - volatile_distance_)>VOLATILE_DISTANCE_THRESHOLD) + { + if (volatile_distance_ < best_volatile_distance) + { + best_volatile_distance = volatile_distance_; + ROS_INFO("Best distance updated"); + best_robot_pose = current_robot_pose; + } + } + + // If the distance is increasing + else if ((last_volatile_distance - volatile_distance_)<-VOLATILE_DISTANCE_THRESHOLD + || !near_volatile_) + { + if(flip_rotation_count < FLIP_ROTATION_COUNT_MAX) + { + ROS_INFO("Flipping Direction"); + rotate_direction = (rotate_direction == CLOCKWISE) ? COUNTERCLOCKWISE : CLOCKWISE; + rotateRobot(rotate_direction); + flip_rotation_count++; + } + else + { + ROS_INFO("Flipped Enough"); + rotate_robot = false; + near_volatile_ = false; + break; + } + } + + last_volatile_distance = volatile_distance_; + } + } + return best_robot_pose; +} + +/** + * @brief Actionlib callback + * + * @param localiser_goal + * @param server + */ +void localiseResource(const operations::ResourceLocaliserGoalConstPtr& localiser_goal, ResourceLocaliserServer* server) +{ + ROS_INFO("Starting locating volatile sequence"); + if (near_volatile_) + { + geometry_msgs::Pose best_robot_pose = getBestPose(); + + ROS_INFO("Setting to best"); + navigateRobot(best_robot_pose); + server->setSucceeded(); + } + else + { + ROS_ERROR("Resourse localisation called, but rover not near volatile"); + server->setAborted(); + } +} + + +/** + * @brief Callback for sensor topic + * + * @param msg + */ +void updateSensorData(const srcp2_msgs::VolSensorMsg::ConstPtr& msg) +{ + new_message_received = true; + if(msg->distance_to == -1) // When there is no volatile nearby, + // Distance is returned as -1 + { + near_volatile_ = false; + volatile_distance_ = MAX_DETECT_DIST + 1; // To make sure any detected + // distance is less than this + } + else + { + near_volatile_ = true; + volatile_distance_ = msg->distance_to; + } +} + +/** + * @brief Subscribes to an odometry topic, and updates the global robot_pose_ + * + * @param msg The odometry message to process + */ +void updateRobotPose(const nav_msgs::Odometry::ConstPtr &msg) +{ + // guard is bad? + robot_pose_.header = msg->header; + robot_pose_.pose = msg->pose.pose; +} + +int main(int argc, char** argv) +{ + // Ensure the robot name is passed in + if (argc != 2 && argc != 4) + { + // Displaying an error message for correct usage of the script, and returning error. + ROS_ERROR_STREAM("Not enough arguments! Please pass in robot name with number."); + return -1; + } + else + { + // Robot Name from argument + std::string robot_name(argv[1]); + std::string node_name = robot_name + "_resource_localiser_action_server"; + ros::init(argc, argv, node_name); + ros::NodeHandle nh; + + ros::Subscriber subscriber = nh.subscribe(robot_name + VOLATILE_SENSOR_TOPIC, 1000, updateSensorData); + ros::Subscriber update_current_robot_pose_ = nh.subscribe(CAPRICORN_TOPIC + robot_name + CHEAT_ODOM_TOPIC, 1000, updateRobotPose); + + ResourceLocaliserServer resource_localiser_server(nh, RESOURCE_LOCALISER_ACTIONLIB, boost::bind(&localiseResource, _1, &resource_localiser_server), false); + resource_localiser_server.start(); + + navigation_client_ = new NavigationClient_(NAVIGATION_ACTIONLIB, true); + + ros::spin(); + + delete navigation_client_; + + return 0; + } +} \ No newline at end of file diff --git a/operations/src/resource_localiser_tester.cpp b/operations/src/resource_localiser_tester.cpp new file mode 100644 index 00000000..8a600673 --- /dev/null +++ b/operations/src/resource_localiser_tester.cpp @@ -0,0 +1,27 @@ +#include // Note: "Action" is appended +#include +#include + +using namespace COMMON_NAMES; + +typedef actionlib::SimpleActionClient ResourceLocaliserClient_; +/** + ***************************************************** + ***** F I L E O N L Y F O R T E S T I N G ***** + ***************************************************** + (in other words, do not consider this to be good code) + */ +int main(int argc, char** argv) +{ + ros::init(argc, argv, "do_dishes_client"); + ResourceLocaliserClient_ resource_localiser_client_(RESOURCE_LOCALISER_ACTIONLIB, true); + resource_localiser_client_.waitForServer(); + operations::ResourceLocaliserGoal goal; + + resource_localiser_client_.sendGoal(goal); + resource_localiser_client_.waitForResult(ros::Duration(15.0)); + if (resource_localiser_client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + printf("Yay! The dishes are now clean"); + printf("Current State: %s\n", resource_localiser_client_.getState().toString().c_str()); + return 0; +} \ No newline at end of file diff --git a/operations/src/scout_state_machine.cpp b/operations/src/scout_state_machine.cpp new file mode 100644 index 00000000..e695bb30 --- /dev/null +++ b/operations/src/scout_state_machine.cpp @@ -0,0 +1,279 @@ +#include +#include + +ScoutStateMachine::ScoutStateMachine(ros::NodeHandle nh, const std::string& robot_name):nh_(nh) +{ + robot_name_ = robot_name; + navigation_client_ = new NavigationClient_(NAVIGATION_ACTIONLIB, true); + resource_localiser_client_ = new ResourceLocaliserClient_(RESOURCE_LOCALISER_ACTIONLIB, true); + + volatile_sub_ = nh.subscribe("/" + robot_name + VOLATILE_SENSOR_TOPIC, 1000, &ScoutStateMachine::processVolatileMessage, this); + + // TODO TODO TODO: Replace (or switch based on debug flat) with real odometry topic + robot_odom_sub_ = nh.subscribe(CAPRICORN_TOPIC + robot_name + CHEAT_ODOM_TOPIC, 1000, &ScoutStateMachine::processOdomMessage, this); + + excavator_ready_sub_ = nh.subscribe(CAPRICORN_TOPIC + EXCAVATOR_ARRIVED_TOPIC, 1000, &ScoutStateMachine::processExcavatorMessage, this); + + volatile_pub_ = nh.advertise(CAPRICORN_TOPIC + robot_name + VOLATILE_LOCATION_TOPIC, 1000); +} + +ScoutStateMachine::~ScoutStateMachine() +{ + delete navigation_client_; + delete resource_localiser_client_; +} + +void ScoutStateMachine::processVolatileMessage(const srcp2_msgs::VolSensorMsg::ConstPtr& vol_msg) +{ + if(vol_msg->vol_type != std::string("none")) + { + std::lock_guard vol_flag_guard(vol_flag_mutex_); + vol_detected_ = true; + } +} + +void ScoutStateMachine::processOdomMessage(const nav_msgs::Odometry::ConstPtr& odom_msg) +{ + std::lock_guard pose_guard(robot_pose_mutex_); + robot_pose_.header = odom_msg->header; + robot_pose_.pose = odom_msg->pose.pose; +} + +void ScoutStateMachine::processExcavatorMessage(const std_msgs::Empty::ConstPtr& odom_msg) +{ + std::lock_guard excavator_ready_guard(excavator_ready_mutex_); + + excavator_ready_ = true; +} + +geometry_msgs::PoseStamped ScoutStateMachine::getRobotPose() +{ + std::lock_guard pose_guard(robot_pose_mutex_); + return robot_pose_; +} + +void ScoutStateMachine::startStateMachine() +{ + navigation_client_->waitForServer(); + + while (ros::ok() && state_machine_continue_) + { + switch (robot_state_) + { + case LOCATOR_STATES::INIT: + { + // Call funtion to go to 'unexplored' region + // For the demo purpose, call goToGoal(20,0) in map frame + + if(first_iter_) + { + printf("Init first iteration: Sending robot to unexplored region.\n"); + + // Demo: unexplored region is a constant location + geometry_msgs::PoseStamped unexplored; + + unexplored.header.frame_id = MAP; + + unexplored.pose.position.x = 40; + unexplored.pose.position.y = 9; + unexplored.pose.position.z = 0; + + unexplored.pose.orientation.w = 1; + unexplored.pose.orientation.x = 0; + unexplored.pose.orientation.y = 0; + unexplored.pose.orientation.z = 0; + + navigation_action_goal_.pose = unexplored; + navigation_action_goal_.drive_mode = NAV_TYPE::GOAL; + + // Send the goal to the navigation server + navigation_client_->sendGoal(navigation_action_goal_); + + first_iter_ = false; + } + + if(navigation_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + printf("Finished going to unexplored region. Beginning search.\n"); + + first_iter_ = true; + + robot_state_ = LOCATOR_STATES::SEARCH; + } + break; + } + case LOCATOR_STATES::SEARCH: + { + // Call a function to execute search algorithm + // For the demo, spiral motion + // (update nav action to enum, call spiral) + // Explicitly terminate nav goal once resource found (volatile topic) + + if(first_iter_) + { + printf("Beginning search for volatiles.\n"); + + // Create a spiral goal. For now, there are no extra parameters passed along. + navigation_action_goal_.drive_mode = NAV_TYPE::SPIRAL; + + // Send the spiral goal to the server + navigation_client_->sendGoal(navigation_action_goal_); + + first_iter_ = false; + + // Lock the volatile flag, and reset the flag to ensure that previous volatile detections are reset + std::lock_guard vol_flag_lock(vol_flag_mutex_); + vol_detected_ = false; + } + + // Lock the vol flag mutex + std::lock_guard vol_flag_lock(vol_flag_mutex_); + + if(vol_detected_) + { + printf("Found a volatile! Localizing...\n"); + + // Cancel the spiral goal + navigation_client_->cancelGoal(); + + first_iter_ = true; + + robot_state_ = LOCATOR_STATES::LOCATE; + } + + break; + } + case LOCATOR_STATES::LOCATE: + { + // Call actionlib for resource_locate + // ResourceLocator + // actionlib call to resource localiser + // resource localiser then interfaces with navigation + + if(first_iter_) + { + printf("Beginning localization.\n"); + + resource_localiser_client_->sendGoal(resource_localiser_goal_); + + first_iter_ = false; + } + + // What if error abort or something else? + + if(resource_localiser_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + printf("Resource localized.\n"); + + first_iter_ = true; + + robot_state_ = LOCATOR_STATES::FOUND; + } + else if(resource_localiser_client_->getState() == actionlib::SimpleClientGoalState::ABORTED) + { + ros::Duration(5.0).sleep(); + ROS_INFO("Retrying to localise the resource"); + resource_localiser_client_->sendGoal(resource_localiser_goal_); + } + break; + } + case LOCATOR_STATES::FOUND: + { + // Stop the robot, brake. + // Publish fake message (for now) for excavator to come and get resource (publish PoseStamped) + // Wait for a flag from excavator that it is ready to take over + + if(first_iter_) + { + printf("Publishing volatile location, then waiting for excavator response.\n"); + + // Publish the most recent robot pose. This should be on top of the volatile. + volatile_pub_.publish(getRobotPose()); + + // Reset the first iteration flag + first_iter_ = false; + + // Lock the flag mutex + std::lock_guard excavator_ready_guard(excavator_ready_mutex_); + + // Reset the excavator ready flag + excavator_ready_ = false; + } + + // Lock the flag mutex + std::lock_guard excavator_ready_guard(excavator_ready_mutex_); + + if(excavator_ready_) + { + printf("Excavator has arrived. Moving out of the way.\n"); + + first_iter_ = true; + + robot_state_ = LOCATOR_STATES::MOVE_OUT; + } + break; + } + case LOCATOR_STATES::MOVE_OUT: + { + // Once flag is received (excavator topic, something), move some distance aside (+/- y) + // For demo, stay in this state (don't return to search) + + if(first_iter_) + { + printf("Moving out of the way.\n"); + + navigation_action_goal_.drive_mode = NAV_TYPE::GOAL; + + // Go 3 meters to the left of the current pose + geometry_msgs::PoseStamped go_left; + + go_left.header.frame_id = robot_name_ + ROBOT_CHASSIS; + + go_left.pose.position.x = 0; + go_left.pose.position.y = 3; + go_left.pose.position.z = 0; + + go_left.pose.orientation.w = 1; + go_left.pose.orientation.x = 0; + go_left.pose.orientation.y = 0; + go_left.pose.orientation.z = 0; + + navigation_action_goal_.pose = go_left; + + navigation_client_->sendGoal(navigation_action_goal_); + + first_iter_ = false; + } + + if(navigation_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED) + { + printf("Done moving out of the way. DEMO: Staying in place, not returning to spiral.\n"); + } + break; + } + case LOCATOR_STATES::RECHARGE: + { + // Mahimana's method to go back to the recharge station + // Find and get to the recharge station + + printf("Recharging! Just kidding, nobody has bothered to implement me yet.\n"); + break; + } + case LOCATOR_STATES::WAIT_FOR_STATE_UPDATE: + { + // This means there is nothing to do. + // Just hang around for any update + + printf("Waiting for a state update.\n"); + break; + } + default: + { + ROS_ERROR_STREAM(robot_name_ + " state machine encountered unhandled state!"); + break; + } + } + + ros::spinOnce(); + } +} \ No newline at end of file diff --git a/operations/src/start_scout_sm.cpp b/operations/src/start_scout_sm.cpp new file mode 100644 index 00000000..8a8f944f --- /dev/null +++ b/operations/src/start_scout_sm.cpp @@ -0,0 +1,24 @@ +#include +#include +#include + +int main(int argc, char* argv[]) +{ + if(argc != 2 && argc != 4) + { + ROS_ERROR_STREAM("This node must be launched with the robotname passed as a command line argument!"); + + return -1; + } + + std::string robot_name(argv[1]); + + ros::init(argc, argv, robot_name + "_sm"); + ros::NodeHandle nh("~"); + + ScoutStateMachine* sm = new ScoutStateMachine(nh, robot_name); + + sm->startStateMachine(); + + printf("Scout state machine died!\n"); +} \ No newline at end of file diff --git a/operations/test/algorithm_tests.cpp b/operations/test/algorithm_tests.cpp index 15764853..1187c333 100644 --- a/operations/test/algorithm_tests.cpp +++ b/operations/test/algorithm_tests.cpp @@ -14,8 +14,33 @@ class HeadingTests :public ::testing::TestWithParampose.orientation.x = 0.0; origin->pose.orientation.y = 0.0; origin->pose.orientation.z = 0.0; - origin->pose.orientation.w = 0.0; + origin->pose.orientation.w = 1.0; target = new geometry_msgs::PoseStamped(); + target->pose.orientation.x = 0.0; + target->pose.orientation.y = 0.0; + target->pose.orientation.z = 0.0; + target->pose.orientation.w = 1.0; + } +}; + +class PositionTests :public ::testing::TestWithParam> { + protected: + // Add handling to test poses in different frames of reference + geometry_msgs::PoseStamped* origin; + geometry_msgs::PoseStamped* target; + double distance; + virtual void SetUp() { + origin = new geometry_msgs::PoseStamped(); + origin->pose.position.x = 0.0; + origin->pose.position.y = 0.0; + origin->pose.position.z = 0.0; + target = new geometry_msgs::PoseStamped(); + target->pose.position.x = 0.0; + target->pose.position.y = 0.0; + target->pose.position.z = 0.0; + + distance = 0.0; } }; @@ -37,6 +62,19 @@ TEST_P(HeadingTests, CheckHeadingDelta) { ASSERT_EQ(expectedYaw, NavigationAlgo::fromQuatToEuler(target)[2]); } +TEST_P(PositionTests, CheckPositionDelta) { + geometry_msgs::PoseStamped* target = new geometry_msgs::PoseStamped(); + target->pose.position.x = std::get<0>(GetParam()); + target->pose.position.y = std::get<1>(GetParam()); + target->pose.position.z = std::get<2>(GetParam()); + geometry_msgs::PoseStamped* origin = new geometry_msgs::PoseStamped(); + origin->pose.position.x = std::get<3>(GetParam()); + origin->pose.position.y = std::get<4>(GetParam()); + origin->pose.position.z = std::get<5>(GetParam()); + double distance = std::get<6>(GetParam()); + ASSERT_EQ(distance, NavigationAlgo::changeInPosition(origin, target)); +} + class QuaternionTests :public ::testing::TestWithParam> { protected: @@ -80,14 +118,20 @@ INSTANTIATE_TEST_CASE_P( HeadingTests, ::testing::Values( std::make_tuple(0,0,0,1,0,0,0,1,0,0,0,0), - // XYZ - // 0.283, -0.711, -2.393 - // ZYX - // 0.296, 0.706, -2.388 std::make_tuple(0,0,0,1,0,0,0,1,0,0,0,0), std::make_tuple(0,0,0,1,0,0,0,1,0,0,0,0)) ); +//Format:(homex,y,z,targetx,y,z,distance) +INSTANTIATE_TEST_CASE_P( + DumbTests, + PositionTests, + ::testing::Values( + std::make_tuple(5,4,0,3,2,0,1.4142135623730951), + std::make_tuple(50,32,0,78,9,0,36.235341863986875), + std::make_tuple(59.545,-74.1052,0,-15.144,-0.007,0,105.20926748266999)) +); + // Run all the tests that were declared with TEST() int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); diff --git a/perception/CMakeLists.txt b/perception/CMakeLists.txt index d7565ab5..7fbd45fc 100755 --- a/perception/CMakeLists.txt +++ b/perception/CMakeLists.txt @@ -183,6 +183,7 @@ target_link_libraries(find_pp_rs_server ${catkin_LIBRARIES} ) + ############# ## Install ## ############# diff --git a/perception/saved_model/old_model/saved_model.pb b/perception/saved_model/old_model/saved_model.pb new file mode 100644 index 00000000..f3ad3421 Binary files /dev/null and b/perception/saved_model/old_model/saved_model.pb differ diff --git a/perception/saved_model/old_model/variables/variables.data-00000-of-00001 b/perception/saved_model/old_model/variables/variables.data-00000-of-00001 new file mode 100644 index 00000000..e18f0a4b Binary files /dev/null and b/perception/saved_model/old_model/variables/variables.data-00000-of-00001 differ diff --git a/perception/saved_model/old_model/variables/variables.index b/perception/saved_model/old_model/variables/variables.index new file mode 100644 index 00000000..365413d4 Binary files /dev/null and b/perception/saved_model/old_model/variables/variables.index differ diff --git a/perception/saved_model_v1/saved_model.pb b/perception/saved_model_v1/saved_model.pb new file mode 100644 index 00000000..eef21f25 Binary files /dev/null and b/perception/saved_model_v1/saved_model.pb differ diff --git a/perception/saved_model_v1/saved_model.pbtxt b/perception/saved_model_v1/saved_model.pbtxt new file mode 100644 index 00000000..2377ff81 --- /dev/null +++ b/perception/saved_model_v1/saved_model.pbtxt @@ -0,0 +1,28 @@ +item { + id: 1 + name: 'processingPlant' +} +item { + id: 2 + name: 'repairStation' +} +item { + id: 3 + name: 'hopper' +} +item { + id: 4 + name: 'rock' +} +item { + id: 5 + name: 'scout' +} +item { + id: 6 + name: 'excavator' +} +item { + id: 7 + name: 'hauler' +} diff --git a/perception/saved_model_v1/variables/variables.data-00000-of-00001 b/perception/saved_model_v1/variables/variables.data-00000-of-00001 new file mode 100644 index 00000000..b4a1f839 Binary files /dev/null and b/perception/saved_model_v1/variables/variables.data-00000-of-00001 differ diff --git a/perception/saved_model_v1/variables/variables.index b/perception/saved_model_v1/variables/variables.index new file mode 100644 index 00000000..71f934b3 Binary files /dev/null and b/perception/saved_model_v1/variables/variables.index differ diff --git a/perception/src/object_detection_cap.py b/perception/src/object_detection_cap.py index 31f174c1..13e5be3d 100755 --- a/perception/src/object_detection_cap.py +++ b/perception/src/object_detection_cap.py @@ -24,7 +24,7 @@ physical_devices = tf.config.list_physical_devices('GPU') tf.config.experimental.set_memory_growth(physical_devices[0], True) -tf.config.experimental.set_virtual_device_configuration(physical_devices[0], [tf.config.experimental.VirtualDeviceConfiguration(memory_limit=800)]) +# tf.config.experimental.set_virtual_device_configuration(physical_devices[0], [tf.config.experimental.VirtualDeviceConfiguration(memory_limit=800)]) bridge = CvBridge() K = [381.36246688113556, 0.0, 320.5, 0.0, 381.36246688113556, 240.5, 0.0, 0.0, 1.0] @@ -225,4 +225,4 @@ def init_object_detection(path_to_model, path_to_label_map): rospy.init_node(robot_name + '_object_detection', anonymous = True) - init_object_detection(path_to_model, path_to_label_map) \ No newline at end of file + init_object_detection(path_to_model, path_to_label_map) diff --git a/utils/include/utils/common_names.h b/utils/include/utils/common_names.h index 5571e665..8f29b5da 100644 --- a/utils/include/utils/common_names.h +++ b/utils/include/utils/common_names.h @@ -16,7 +16,8 @@ namespace COMMON_NAMES /****** ROBOT FRAMES ******/ const std::string MAP = "map"; const std::string ODOM = "odom"; - const std::string ROBOT_BASE = "base_footprint"; + const std::string ROBOT_BASE = "base_footprint"; + const std::string ROBOT_CHASSIS = "_small_chassis"; /****** WHEELS ******/ const std::string FRONT_LEFT_WHEEL = "/front_left_wheel"; @@ -33,6 +34,9 @@ namespace COMMON_NAMES /****** ACTIONLIBS ******/ const std::string NAVIGATION_ACTIONLIB = "navigation"; + const std::string RESOURCE_LOCALISER_ACTIONLIB = "resource_localiser_actionlib"; + const std::string HAULER_ACTIONLIB = "hauler_bin"; + const std::string EXCAVATOR_ACTIONLIB = "excavator"; /****** GAZEBO ******/ const std::string HEIGHTMAP = "heightmap"; @@ -51,6 +55,7 @@ namespace COMMON_NAMES const std::string FIND_PP_RS_ACTIONLIB_NAME = "_find_pp_rs"; /****** ROS NODE NAMES ******/ + const std::string NAVIGATION_VISION_NODE_NAME = "_navigation_vision"; const std::string PR_DATASET_NODE_NAME = "_pr_dataset"; const std::string GROUND_TRUTH_PR_NODE_NAME = "_ground_truth_pr"; const std::string PR_LOCALIZATION_NODE_NAME = "_pr_localization"; @@ -72,15 +77,39 @@ namespace COMMON_NAMES const std::string RIGHT_CAMERAINFO_TOPIC = "/camera/right/camera_info"; const std::string LEFT_CAMERAINFO_TOPIC = "/camera/left/camera_info"; const std::string SET_SENSOR_PITCH_TOPIC = "/sensor/pitch/command/position"; + + /****** HAULER NAMES ******/ + const std::string SET_BIN_POSITION = "/bin/command/position"; + + /****** EXCAVATOR NAMES ******/ + const std::string SET_ELBOW_PITCH_POSITION = "/arm/elbow_pitch/command/position"; + const std::string SET_SHOULDER_PITCH_POSITION = "/arm/shoulder_pitch/command/position"; + const std::string SET_SHOULDER_YAW_POSITION = "/arm/shoulder_yaw/command/position"; + const std::string SET_WRIST_PITCH_POSITION = "/arm/wrist_pitch/command/position"; + const std::string WHEEL_PID = "/wheel_pid"; const std::string SET_SENSOR_YAW_TOPIC = "/sensor/yaw/command/position"; const std::string OBJECT_DETECTION_OBJECTS_TOPIC = "/object_detection/objects"; + const std::string VOLATILE_SENSOR_TOPIC = "/volatile_sensor"; + const std::string VOLATILE_LOCATION_TOPIC = "/volatile_location"; + + // Used to communicate between excavators and scouts when the excavator is ready to move in to pick up a volatile + // TODO: Choose a real message type for this topic, instead of std_msgs::Empty + const std::string EXCAVATOR_ARRIVED_TOPIC = "/excavator_arrived"; /****** OBJECT DETECTION CLASS NAMES ******/ const std::string OBJECT_DETECTION_PROCESSING_PLANT_CLASS = "processingPlant"; const std::string OBJECT_DETECTION_REPAIR_STATION_CLASS = "repairStation"; - /****** NAVIGATION ACTION RESULT ENUM ******/ + /****** NAVIGATION ENUMS ******/ + enum NAV_TYPE + { + MANUAL = 0, // Manual driving + GOAL = 1, // Trajectory generation with the planner from a goal + SPIRAL = 2, // Archimedean spiral (scout finding volatiles) + FOLLOW = 3, // Follow an object in frame + }; + enum NAV_RESULT { FAILED = 0, @@ -88,5 +117,4 @@ namespace COMMON_NAMES INTERRUPTED = 2 }; - } // namespace CAPRICORN_COMMON_NAMES