bool MoveObject::goObject() { //connet to the Server, 5s limit while (!move_base.waitForServer(ros::Duration(5.0))) { ROS_INFO("Waiting for move_base action server..."); } ROS_INFO("Connected to move base server"); /t the targetpose move_base_msgs::MoveBaseGoal goal; goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); // goal.target_pose.pose.position.x = Obj_pose.pose.position.x; // goal.target_pose.pose.position.y = Obj_pose.pose.position.y; // target_odom_point.pose.pose.position.x=goal.target_pose.pose.position.x // target_odom_point.pose.pose.position.y=goal.target_pose.pose.position.y target_odom_point.pose.pose.position.x=Obj_pose.pose.position.x; target_odom_point.pose.pose.position.y=Obj_pose.pose.position.y; cout << goal.target_pose.pose.position.x << endl; cout << goal.target_pose.pose.position.y << endl; //goal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(g.response.yaw); goal.target_pose.pose.orientation.z = 0.0; goal.target_pose.pose.orientation.w = 1.0; tf::quaternionMsgToTF(target_odom_point.pose.orientation, quat); tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换 yaw +=1.5708;//旋转90 target_odom_point.pose.position.x -=keep_distance*cos(yaw); target_odom_point.pose.position.y -=keep_distance*sin(yaw); goal.target_pose.pose.position.x=target_odom_point.pose.pose.position.x goal.target_pose.pose.position.y=target_odom_point.pose.pose.position.y target_odom_point.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); ROS_INFO("Sending goal"); move_base.sendGoal(goal); move_base.waitForResult(); if (move_base.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { ROS_INFO("Goal succeeded!"); return true; } else { ROS_INFO("Goal failed"); return false; } }
时间: 2023-12-20 11:04:19 浏览: 40
这段代码是一个移动机器人到目标点的函数。首先会等待 move_base action server 的连接,如果在 5 秒内没有连接成功,则会输出 "Waiting for move_base action server..."。如果连接成功,则会输出 "Connected to move base server"。
接下来,设置目标点的坐标和姿态,这里的目标点是以地图为参考系的。然后通过计算机器人需要旋转的角度,将目标点向机器人前进方向偏移一定距离,即目标点距离机器人保持一定距离。最后通过 move_base.sendGoal(goal) 发送目标点,等待机器人到达目标点,如果成功到达则返回 true,否则返回 false。
相关问题
读取#@brief: gerneral config parameters general: use_ros: true #@type: bool @detail: flag to control using ROS functions or not # if ture, you can send or receive messages through ROS (different messages need to be config in their own yaml file separatedly) use_proto: true #@type: bool @detail: flag to control using Proto functions or not # if ture, you can send or receive sensor: #@brief: config file for Imu message imu: include: ../sensor_config/other_sensors/rs_imu.yaml #@brief: config file for Gnss message gnss: include: ../sensor_config/other_sensors/rs_gnss.yaml #@brief: config file for Odom message odom: include: ../sensor_config/other_sensors/rs_odom.yaml #@brief: config file for Lidar message lidar: include: ../sensor_config/lidar/rs_lidar.yaml #@brief: config file for Lidar message camera: include: ../sensor_config/other_sensors/rs_camera.yaml #@brief: config file for Radar message radar: include: ../sensor_config/other_sensors/rs_radar.yaml
要读取上述YAML文件,可以使用适合的YAML解析库,例如YAML-CPP或YAMLCPP-Parser。以下是使用YAML-CPP的示例代码:
```cpp
#include <iostream>
#include <yaml-cpp/yaml.h>
void readConfig(const YAML::Node& config) {
if (config["general"]) {
// 读取general字段的值
YAML::Node general = config["general"];
if (general["use_ros"]) {
bool useRos = general["use_ros"].as<bool>();
std::cout << "use_ros: " << std::boolalpha << useRos << std::endl;
}
if (general["use_proto"]) {
bool useProto = general["use_proto"].as<bool>();
std::cout << "use_proto: " << std::boolalpha << useProto << std::endl;
}
}
if (config["sensor"]) {
// 读取sensor字段的值
YAML::Node sensor = config["sensor"];
if (sensor["imu"]) {
std::string imuInclude = sensor["imu"]["include"].as<std::string>();
std::cout << "imu include: " << imuInclude << std::endl;
}
if (sensor["gnss"]) {
std::string gnssInclude = sensor["gnss"]["include"].as<std::string>();
std::cout << "gnss include: " << gnssInclude << std::endl;
}
if (sensor["odom"]) {
std::string odomInclude = sensor["odom"]["include"].as<std::string>();
std::cout << "odom include: " << odomInclude << std::endl;
}
if (sensor["lidar"]) {
std::string lidarInclude = sensor["lidar"]["include"].as<std::string>();
std::cout << "lidar include: " << lidarInclude << std::endl;
}
if (sensor["camera"]) {
std::string cameraInclude = sensor["camera"]["include"].as<std::string>();
std::cout << "camera include: " << cameraInclude << std::endl;
}
if (sensor["radar"]) {
std::string radarInclude = sensor["radar"]["include"].as<std::string>();
std::cout << "radar include: " << radarInclude << std::endl;
}
}
}
int main() {
// 加载YAML文件
YAML::Node config = YAML::LoadFile("config.yaml");
// 读取配置
readConfig(config);
return 0;
}
```
请确保将上述示例代码与YAML-CPP库一起编译、链接并运行。从YAML文件中读取的值将打印到控制台上。
读取ymal 文件common: msg_source: 1 #@type: int @detail: 0--not use Camera, 1--message come from sensor driver, 2--message come from ROS,3--message come from protobuf(UDP) send_msg_ros: true #@type: bool @detail: if true, the original Gnss message will be sent out through ROS send_msg_proto: false #@type: bool @detail: if true, the original Gnss message will be sent out through Proto camera_number: 4 camera_paths: camera0: include: ../sensor_config/other_sensors/camera/camera_C920e1.yaml camera1: include: ../sensor_config/other_sensors/camera/camera_C920e2.yaml camera2: include: ../sensor_config/other_sensors/camera/camera_C920e3.yaml camera3: include: ../sensor_config/other_sensors/camera/camera_C920e4.yaml
要读取上述YAML文件,您可以使用适合的YAML解析库,例如YAML-CPP或YAMLCPP-Parser。下面是使用YAML-CPP的示例代码:
```cpp
#include <iostream>
#include <yaml-cpp/yaml.h>
int main() {
// 加载YAML文件
YAML::Node config = YAML::LoadFile("common.yaml");
// 读取msg_source字段的值
int msgSource = config["msg_source"].as<int>();
std::cout << "msg_source: " << msgSource << std::endl;
// 读取send_msg_ros字段的值
bool sendMsgRos = config["send_msg_ros"].as<bool>();
std::cout << "send_msg_ros: " << std::boolalpha << sendMsgRos << std::endl;
// 读取send_msg_proto字段的值
bool sendMsgProto = config["send_msg_proto"].as<bool>();
std::cout << "send_msg_proto: " << std::boolalpha << sendMsgProto << std::endl;
// 读取camera_number字段的值
int cameraNumber = config["camera_number"].as<int>();
std::cout << "camera_number: " << cameraNumber << std::endl;
// 读取camera_paths字段的值
YAML::Node cameraPaths = config["camera_paths"];
for (const auto& camera : cameraPaths) {
std::string cameraName = camera.first.as<std::string>();
std::string includePath = camera.second["include"].as<std::string>();
std::cout << "camera name: " << cameraName << ", include path: " << includePath << std::endl;
}
return 0;
}
```
请确保将上述示例代码与YAML-CPP库一起编译,链接并运行。从YAML文件中读取的值将打印到控制台上。