280 lines
9.2 KiB
C++
280 lines
9.2 KiB
C++
|
|
#include <ros/ros.h>
|
|
#include <pluginlib/class_loader.h>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <move_base_core/navigation.h>
|
|
#include <amr_control/moveTo.h>
|
|
#include <std_msgs/Bool.h>
|
|
#include <std_msgs/Int16.h>
|
|
#include <geometry_msgs/Vector3.h>
|
|
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
|
#include <geometry_msgs/Pose2D.h>
|
|
#include <nav_2d_utils/conversions.h>
|
|
#include <nav_2d_utils/tf_help.h>
|
|
#include <nav_2d_utils/path_ops.h>
|
|
#include <vda5050_msgs/Order.h>
|
|
#include <vda5050_msgs/InstantActions.h>
|
|
|
|
#include <tf2/convert.h>
|
|
#include <tf2/utils.h>
|
|
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
|
#include <tf2_ros/transform_listener.h>
|
|
#include <geometry_msgs/Pose2D.h>
|
|
|
|
move_base_core::BaseNavigation::Ptr move_base = nullptr;
|
|
geometry_msgs::PoseStamped amcl_pose;
|
|
boost::mutex ctr_mutex;
|
|
bool pub_order = false;
|
|
amr_control::moveTo order;
|
|
bool pub_cancel = false;
|
|
|
|
void moveToCallback(const amr_control::moveTo::ConstPtr& msg)
|
|
{
|
|
// boost::unique_lock<boost::mutex> lock(ctr_mutex);
|
|
if(move_base)
|
|
{
|
|
try
|
|
{
|
|
geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(msg->goal, "map", ros::Time::now());
|
|
geometry_msgs::PoseStamped of_goal = move_base_core::offset_goal(amcl_pose, msg->goal.x);
|
|
double xy_goal_tolerance = 0.04;
|
|
double yaw_goal_tolerance = 0.06;
|
|
// ROS_INFO_STREAM(of_goal);
|
|
switch ((int)msg->maker.data)
|
|
{
|
|
case 1:
|
|
move_base->moveTo(goal, move_base_core::PICK_UP, xy_goal_tolerance, yaw_goal_tolerance);
|
|
break;
|
|
case 2:
|
|
move_base->moveTo(goal, move_base_core::POP_DOWN, xy_goal_tolerance, yaw_goal_tolerance);
|
|
break;
|
|
case 3:
|
|
move_base->moveTo(goal, move_base_core::POSITION, xy_goal_tolerance, yaw_goal_tolerance);
|
|
break;
|
|
case 4:
|
|
move_base->moveTo(goal, move_base_core::CHARGER, xy_goal_tolerance, yaw_goal_tolerance);
|
|
break;
|
|
case 5:
|
|
move_base->moveStraightTo(of_goal, xy_goal_tolerance);
|
|
break;
|
|
case 6:
|
|
move_base->rotateTo(goal, yaw_goal_tolerance);
|
|
break;
|
|
default:
|
|
throw std::bad_function_call();
|
|
break;
|
|
}
|
|
|
|
geometry_msgs::Vector3 linear;
|
|
linear.x = 1.1;
|
|
move_base->setTwistLinear(linear);
|
|
linear.x = -0.5;
|
|
move_base->setTwistLinear(linear);
|
|
}
|
|
catch(const std::exception& e)
|
|
{
|
|
ROS_ERROR_STREAM(e.what() << "\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
void getposeCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
|
|
{
|
|
amcl_pose.header = msg->header;
|
|
amcl_pose.pose.position = msg->pose.pose.position;
|
|
amcl_pose.pose.orientation = msg->pose.pose.orientation;
|
|
}
|
|
|
|
void setTiwstCallback(const geometry_msgs::Vector3::ConstPtr& msg)
|
|
{
|
|
if(move_base)
|
|
{
|
|
move_base->setTwistLinear(*msg);
|
|
}
|
|
}
|
|
|
|
void cancelCallback(const std_msgs::Bool::ConstPtr& msg)
|
|
{
|
|
move_base->cancel();
|
|
}
|
|
|
|
void pauseCallback(const std_msgs::Bool::ConstPtr& msg)
|
|
{
|
|
move_base->pause();
|
|
}
|
|
|
|
void resumeCallback(const std_msgs::Bool::ConstPtr& msg)
|
|
{
|
|
move_base->resume();
|
|
}
|
|
|
|
void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg)
|
|
{
|
|
if(msg->serialNumber == "AMR-150")
|
|
{
|
|
ros::Duration(0.5).sleep();
|
|
if(!msg->nodes.empty())
|
|
{
|
|
order.header.frame_id = "map";
|
|
order.header.stamp = ros::Time::now();
|
|
order.goal.x = msg->nodes.back().nodePosition.x;
|
|
order.goal.y = msg->nodes.back().nodePosition.y;
|
|
order.goal.theta = msg->nodes.back().nodePosition.theta;
|
|
if(!msg->nodes.back().actions.empty())
|
|
{
|
|
if(msg->nodes.back().actions.front().actionType == "startInPallet")
|
|
order.maker.data = 1;
|
|
else
|
|
order.maker.data = 3;
|
|
}
|
|
else
|
|
order.maker.data = 3;
|
|
pub_order = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
void instantActions_msg_handle(const vda5050_msgs::InstantActions::ConstPtr& msg)
|
|
{
|
|
if(msg->serialNumber == "AMR-150")
|
|
{
|
|
for(int i=0; i< msg->actions.size(); i++)
|
|
{
|
|
if(msg->actions[i].actionType == "cancelOrder") pub_cancel = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool getRobotPose(geometry_msgs::PoseStamped& global_pose, std::shared_ptr<tf2_ros::Buffer> tf_)
|
|
{
|
|
std::string global_frame_ = "map";
|
|
std::string robot_base_frame_ = "base_link";
|
|
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
|
|
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
|
|
geometry_msgs::PoseStamped robot_pose;
|
|
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
|
|
robot_pose.header.frame_id = robot_base_frame_;
|
|
robot_pose.header.stamp = ros::Time();
|
|
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
|
|
|
|
// get the global pose of the robot
|
|
try
|
|
{
|
|
// use current time if possible (makes sure it's not in the future)
|
|
if (tf_->canTransform(global_frame_, robot_base_frame_, current_time))
|
|
{
|
|
geometry_msgs::TransformStamped transform = tf_->lookupTransform(global_frame_, robot_base_frame_, current_time);
|
|
tf2::doTransform(robot_pose, global_pose, transform);
|
|
}
|
|
// use the latest otherwise
|
|
else
|
|
{
|
|
tf_->transform(robot_pose, global_pose, global_frame_);
|
|
}
|
|
}
|
|
catch (tf2::LookupException& ex)
|
|
{
|
|
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
|
|
return false;
|
|
}
|
|
catch (tf2::ConnectivityException& ex)
|
|
{
|
|
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
|
|
return false;
|
|
}
|
|
catch (tf2::ExtrapolationException& ex)
|
|
{
|
|
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
|
|
return false;
|
|
}
|
|
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
|
|
// check global_pose timeout
|
|
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > 1.0)
|
|
{
|
|
ROS_WARN_THROTTLE(1.0,
|
|
// ROS_WARN(
|
|
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
|
|
current_time.toSec(), global_pose.header.stamp.toSec(), 1.0);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv){
|
|
ros::init(argc, argv, "move_base_node");
|
|
ros::NodeHandle nh = ros::NodeHandle("move_base_node");
|
|
ros::NodeHandle nh_;
|
|
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
|
|
tf2_ros::TransformListener tf2(*buffer);
|
|
|
|
pluginlib::ClassLoader<move_base_core::BaseNavigation> mb_loader("move_base_core", "move_base_core::BaseNavigation");
|
|
|
|
std::string obj_name("move_base::MoveBase");
|
|
try
|
|
{
|
|
move_base = mb_loader.createUniqueInstance(obj_name);
|
|
ROS_INFO("Created object %s susseced", obj_name.c_str());
|
|
move_base->initialize(buffer);
|
|
}
|
|
catch (pluginlib::PluginlibException &ex)
|
|
{
|
|
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
|
|
return 1;
|
|
}
|
|
catch(std::exception &e)
|
|
{
|
|
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
|
|
return 1;
|
|
}
|
|
ros::Subscriber moveto_sub = nh.subscribe("moveTo", 1000, moveToCallback);
|
|
ros::Subscriber set_twist_sub = nh.subscribe("setTiwst", 1000, setTiwstCallback);
|
|
ros::Subscriber cancel_sub = nh.subscribe("cancel", 1000, cancelCallback);
|
|
ros::Subscriber pause_sub = nh.subscribe("pause", 1000, pauseCallback);
|
|
ros::Subscriber resume_sub = nh.subscribe("resume", 1000, resumeCallback);
|
|
ros::Subscriber amcl_pose_sub = nh.subscribe("amcl_pose", 1000, getposeCallback);
|
|
ros::Subscriber order_msg_sub_ = nh_.subscribe("/order",1000, order_msg_handle);
|
|
ros::Subscriber instantAc_msg_sub_ = nh_.subscribe("/instantActions",1000, instantActions_msg_handle);
|
|
ros::Publisher moveto_pub = nh.advertise<amr_control::moveTo>("moveTo", 1000);
|
|
ros::Publisher cancel_pub = nh.advertise<std_msgs::Bool>("cancel", 1000);
|
|
ros::Rate r(10);
|
|
|
|
while (ros::ok())
|
|
{
|
|
if(pub_order)
|
|
{
|
|
moveto_pub.publish(order);
|
|
pub_order = false;
|
|
}
|
|
else if (pub_cancel)
|
|
{
|
|
std_msgs::Bool value;
|
|
value.data = true;
|
|
cancel_pub.publish(value);
|
|
pub_cancel = false;
|
|
}
|
|
getRobotPose(amcl_pose, buffer);
|
|
// boost::unique_lock<boost::mutex> lock(ctr_mutex);
|
|
// {
|
|
if(move_base && move_base->nav_feedback_)
|
|
{
|
|
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::ABORTED)
|
|
// ROS_ERROR("ABORTED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
|
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::PAUSED)
|
|
// ROS_WARN("PAUSED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
|
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::PREEMPTED)
|
|
// ROS_WARN("PREEMPTED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
|
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::SUCCEEDED)
|
|
// ROS_INFO_STREAM("SUCCEEDED \n" << move_base->nav_feedback_->current_pose);
|
|
}
|
|
// }
|
|
ros::spinOnce();
|
|
r.sleep();
|
|
}
|
|
|
|
ros::spin();
|
|
return(0);
|
|
}
|