28 lines
1.2 KiB
C++
28 lines
1.2 KiB
C++
#include "amr_control/amr_subcriber.h"
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
|
|
amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
|
|
: nh_(nh), move_base_ptr_(move_base_ptr)
|
|
{
|
|
robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str());
|
|
ros::NodeHandle nh_move_base_simple;
|
|
move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
|
|
}
|
|
|
|
amr_control::AmrSubscriber::~AmrSubscriber()
|
|
{
|
|
}
|
|
|
|
void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
|
|
{
|
|
robot_geometry_msgs::PoseStamped goal;
|
|
goal.header.stamp = robot::Time::now();
|
|
goal.header.frame_id = msg->header.frame_id;
|
|
goal.pose.position.x = msg->pose.position.x;
|
|
goal.pose.position.y = msg->pose.position.y;
|
|
goal.pose.orientation.x = msg->pose.orientation.x;
|
|
goal.pose.orientation.y = msg->pose.orientation.y;
|
|
goal.pose.orientation.z = msg->pose.orientation.z;
|
|
goal.pose.orientation.w = msg->pose.orientation.w;
|
|
move_base_ptr_->moveTo(goal, 0.02, 0.02);
|
|
} |