p5akopqw4
/
senjing
1
0
Fork 0
Code Issues Pull Requests 2 Projects Releases Wiki Activity

update

Browse Source
xiaojinchi_branch
xiao 1 year ago
parent 3307a2e8ae
commit 097f10b44b
5 changed files with 3785 additions and 0 deletions
Show Stats Download Patch File Download Diff File

3119
src/src/cJSON.c
View File

File diff suppressed because it is too large Load Diff

300
src/src/cJSON.h
Unescape View File

@ -0,0 +1,300 @@
/*
Copyright (c) 2009-2017 Dave Gamble and cJSON contributors
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#ifndef cJSON__h
#define cJSON__h
#ifdef __cplusplus
extern "C"
{
#endif
#if !defined(__WINDOWS__) && (defined(WIN32) || defined(WIN64) || defined(_MSC_VER) || defined(_WIN32))
#define __WINDOWS__
#endif
#ifdef __WINDOWS__
/* When compiling for windows, we specify a specific calling convention to avoid issues where we are being called from a project with a different default calling convention. For windows you have 3 define options:
CJSON_HIDE_SYMBOLS - Define this in the case where you don't want to ever dllexport symbols
CJSON_EXPORT_SYMBOLS - Define this on library build when you want to dllexport symbols (default)
CJSON_IMPORT_SYMBOLS - Define this if you want to dllimport symbol
For *nix builds that support visibility attribute, you can define similar behavior by
setting default visibility to hidden by adding
-fvisibility=hidden (for gcc)
or
-xldscope=hidden (for sun cc)
to CFLAGS
then using the CJSON_API_VISIBILITY flag to "export" the same symbols the way CJSON_EXPORT_SYMBOLS does
*/
#define CJSON_CDECL __cdecl
#define CJSON_STDCALL __stdcall
/* export symbols by default, this is necessary for copy pasting the C and header file */
#if !defined(CJSON_HIDE_SYMBOLS) && !defined(CJSON_IMPORT_SYMBOLS) && !defined(CJSON_EXPORT_SYMBOLS)
#define CJSON_EXPORT_SYMBOLS
#endif
#if defined(CJSON_HIDE_SYMBOLS)
#define CJSON_PUBLIC(type) type CJSON_STDCALL
#elif defined(CJSON_EXPORT_SYMBOLS)
#define CJSON_PUBLIC(type) __declspec(dllexport) type CJSON_STDCALL
#elif defined(CJSON_IMPORT_SYMBOLS)
#define CJSON_PUBLIC(type) __declspec(dllimport) type CJSON_STDCALL
#endif
#else /* !__WINDOWS__ */
#define CJSON_CDECL
#define CJSON_STDCALL
#if (defined(__GNUC__) || defined(__SUNPRO_CC) || defined (__SUNPRO_C)) && defined(CJSON_API_VISIBILITY)
#define CJSON_PUBLIC(type) __attribute__((visibility("default"))) type
#else
#define CJSON_PUBLIC(type) type
#endif
#endif
/* project version */
#define CJSON_VERSION_MAJOR 1
#define CJSON_VERSION_MINOR 7
#define CJSON_VERSION_PATCH 15
#include <stddef.h>
/* cJSON Types: */
#define cJSON_Invalid (0)
#define cJSON_False (1 << 0)
#define cJSON_True (1 << 1)
#define cJSON_NULL (1 << 2)
#define cJSON_Number (1 << 3)
#define cJSON_String (1 << 4)
#define cJSON_Array (1 << 5)
#define cJSON_Object (1 << 6)
#define cJSON_Raw (1 << 7) /* raw json */
#define cJSON_IsReference 256
#define cJSON_StringIsConst 512
/* The cJSON structure: */
typedef struct cJSON
{
/* next/prev allow you to walk array/object chains. Alternatively, use GetArraySize/GetArrayItem/GetObjectItem */
struct cJSON *next;
struct cJSON *prev;
/* An array or object item will have a child pointer pointing to a chain of the items in the array/object. */
struct cJSON *child;
/* The type of the item, as above. */
int type;
/* The item's string, if type==cJSON_String and type == cJSON_Raw */
char *valuestring;
/* writing to valueint is DEPRECATED, use cJSON_SetNumberValue instead */
int valueint;
/* The item's number, if type==cJSON_Number */
double valuedouble;
/* The item's name string, if this item is the child of, or is in the list of subitems of an object. */
char *string;
} cJSON;
typedef struct cJSON_Hooks
{
/* malloc/free are CDECL on Windows regardless of the default calling convention of the compiler, so ensure the hooks allow passing those functions directly. */
void *(CJSON_CDECL *malloc_fn)(size_t sz);
void (CJSON_CDECL *free_fn)(void *ptr);
} cJSON_Hooks;
typedef int cJSON_bool;
/* Limits how deeply nested arrays/objects can be before cJSON rejects to parse them.
* This is to prevent stack overflows. */
#ifndef CJSON_NESTING_LIMIT
#define CJSON_NESTING_LIMIT 1000
#endif
/* returns the version of cJSON as a string */
CJSON_PUBLIC(const char*) cJSON_Version(void);
/* Supply malloc, realloc and free functions to cJSON */
CJSON_PUBLIC(void) cJSON_InitHooks(cJSON_Hooks* hooks);
/* Memory Management: the caller is always responsible to free the results from all variants of cJSON_Parse (with cJSON_Delete) and cJSON_Print (with stdlib free, cJSON_Hooks.free_fn, or cJSON_free as appropriate). The exception is cJSON_PrintPreallocated, where the caller has full responsibility of the buffer. */
/* Supply a block of JSON, and this returns a cJSON object you can interrogate. */
CJSON_PUBLIC(cJSON *) cJSON_Parse(const char *value);
CJSON_PUBLIC(cJSON *) cJSON_ParseWithLength(const char *value, size_t buffer_length);
/* ParseWithOpts allows you to require (and check) that the JSON is null terminated, and to retrieve the pointer to the final byte parsed. */
/* If you supply a ptr in return_parse_end and parsing fails, then return_parse_end will contain a pointer to the error so will match cJSON_GetErrorPtr(). */
CJSON_PUBLIC(cJSON *) cJSON_ParseWithOpts(const char *value, const char **return_parse_end, cJSON_bool require_null_terminated);
CJSON_PUBLIC(cJSON *) cJSON_ParseWithLengthOpts(const char *value, size_t buffer_length, const char **return_parse_end, cJSON_bool require_null_terminated);
/* Render a cJSON entity to text for transfer/storage. */
CJSON_PUBLIC(char *) cJSON_Print(const cJSON *item);
/* Render a cJSON entity to text for transfer/storage without any formatting. */
CJSON_PUBLIC(char *) cJSON_PrintUnformatted(const cJSON *item);
/* Render a cJSON entity to text using a buffered strategy. prebuffer is a guess at the final size. guessing well reduces reallocation. fmt=0 gives unformatted, =1 gives formatted */
CJSON_PUBLIC(char *) cJSON_PrintBuffered(const cJSON *item, int prebuffer, cJSON_bool fmt);
/* Render a cJSON entity to text using a buffer already allocated in memory with given length. Returns 1 on success and 0 on failure. */
/* NOTE: cJSON is not always 100% accurate in estimating how much memory it will use, so to be safe allocate 5 bytes more than you actually need */
CJSON_PUBLIC(cJSON_bool) cJSON_PrintPreallocated(cJSON *item, char *buffer, const int length, const cJSON_bool format);
/* Delete a cJSON entity and all subentities. */
CJSON_PUBLIC(void) cJSON_Delete(cJSON *item);
/* Returns the number of items in an array (or object). */
CJSON_PUBLIC(int) cJSON_GetArraySize(const cJSON *array);
/* Retrieve item number "index" from array "array". Returns NULL if unsuccessful. */
CJSON_PUBLIC(cJSON *) cJSON_GetArrayItem(const cJSON *array, int index);
/* Get item "string" from object. Case insensitive. */
CJSON_PUBLIC(cJSON *) cJSON_GetObjectItem(const cJSON * const object, const char * const string);
CJSON_PUBLIC(cJSON *) cJSON_GetObjectItemCaseSensitive(const cJSON * const object, const char * const string);
CJSON_PUBLIC(cJSON_bool) cJSON_HasObjectItem(const cJSON *object, const char *string);
/* For analysing failed parses. This returns a pointer to the parse error. You'll probably need to look a few chars back to make sense of it. Defined when cJSON_Parse() returns 0. 0 when cJSON_Parse() succeeds. */
CJSON_PUBLIC(const char *) cJSON_GetErrorPtr(void);
/* Check item type and return its value */
CJSON_PUBLIC(char *) cJSON_GetStringValue(const cJSON * const item);
CJSON_PUBLIC(double) cJSON_GetNumberValue(const cJSON * const item);
/* These functions check the type of an item */
CJSON_PUBLIC(cJSON_bool) cJSON_IsInvalid(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsFalse(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsTrue(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsBool(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsNull(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsNumber(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsString(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsArray(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsObject(const cJSON * const item);
CJSON_PUBLIC(cJSON_bool) cJSON_IsRaw(const cJSON * const item);
/* These calls create a cJSON item of the appropriate type. */
CJSON_PUBLIC(cJSON *) cJSON_CreateNull(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateTrue(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateFalse(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateBool(cJSON_bool boolean);
CJSON_PUBLIC(cJSON *) cJSON_CreateNumber(double num);
CJSON_PUBLIC(cJSON *) cJSON_CreateString(const char *string);
/* raw json */
CJSON_PUBLIC(cJSON *) cJSON_CreateRaw(const char *raw);
CJSON_PUBLIC(cJSON *) cJSON_CreateArray(void);
CJSON_PUBLIC(cJSON *) cJSON_CreateObject(void);
/* Create a string where valuestring references a string so
* it will not be freed by cJSON_Delete */
CJSON_PUBLIC(cJSON *) cJSON_CreateStringReference(const char *string);
/* Create an object/array that only references it's elements so
* they will not be freed by cJSON_Delete */
CJSON_PUBLIC(cJSON *) cJSON_CreateObjectReference(const cJSON *child);
CJSON_PUBLIC(cJSON *) cJSON_CreateArrayReference(const cJSON *child);
/* These utilities create an Array of count items.
* The parameter count cannot be greater than the number of elements in the number array, otherwise array access will be out of bounds.*/
CJSON_PUBLIC(cJSON *) cJSON_CreateIntArray(const int *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateFloatArray(const float *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateDoubleArray(const double *numbers, int count);
CJSON_PUBLIC(cJSON *) cJSON_CreateStringArray(const char *const *strings, int count);
/* Append item to the specified array/object. */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToArray(cJSON *array, cJSON *item);
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObject(cJSON *object, const char *string, cJSON *item);
/* Use this when string is definitely const (i.e. a literal, or as good as), and will definitely survive the cJSON object.
* WARNING: When this function was used, make sure to always check that (item->type & cJSON_StringIsConst) is zero before
* writing to `item->string` */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemToObjectCS(cJSON *object, const char *string, cJSON *item);
/* Append reference to item to the specified array/object. Use this when you want to add an existing cJSON to a new cJSON, but don't want to corrupt your existing cJSON. */
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToArray(cJSON *array, cJSON *item);
CJSON_PUBLIC(cJSON_bool) cJSON_AddItemReferenceToObject(cJSON *object, const char *string, cJSON *item);
/* Remove/Detach items from Arrays/Objects. */
CJSON_PUBLIC(cJSON *) cJSON_DetachItemViaPointer(cJSON *parent, cJSON * const item);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromArray(cJSON *array, int which);
CJSON_PUBLIC(void) cJSON_DeleteItemFromArray(cJSON *array, int which);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObject(cJSON *object, const char *string);
CJSON_PUBLIC(cJSON *) cJSON_DetachItemFromObjectCaseSensitive(cJSON *object, const char *string);
CJSON_PUBLIC(void) cJSON_DeleteItemFromObject(cJSON *object, const char *string);
CJSON_PUBLIC(void) cJSON_DeleteItemFromObjectCaseSensitive(cJSON *object, const char *string);
/* Update array items. */
CJSON_PUBLIC(cJSON_bool) cJSON_InsertItemInArray(cJSON *array, int which, cJSON *newitem); /* Shifts pre-existing items to the right. */
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemViaPointer(cJSON * const parent, cJSON * const item, cJSON * replacement);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInArray(cJSON *array, int which, cJSON *newitem);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObject(cJSON *object,const char *string,cJSON *newitem);
CJSON_PUBLIC(cJSON_bool) cJSON_ReplaceItemInObjectCaseSensitive(cJSON *object,const char *string,cJSON *newitem);
/* Duplicate a cJSON item */
CJSON_PUBLIC(cJSON *) cJSON_Duplicate(const cJSON *item, cJSON_bool recurse);
/* Duplicate will create a new, identical cJSON item to the one you pass, in new memory that will
* need to be released. With recurse!=0, it will duplicate any children connected to the item.
* The item->next and ->prev pointers are always zero on return from Duplicate. */
/* Recursively compare two cJSON items for equality. If either a or b is NULL or invalid, they will be considered unequal.
* case_sensitive determines if object keys are treated case sensitive (1) or case insensitive (0) */
CJSON_PUBLIC(cJSON_bool) cJSON_Compare(const cJSON * const a, const cJSON * const b, const cJSON_bool case_sensitive);
/* Minify a strings, remove blank characters(such as ' ', '\t', '\r', '\n') from strings.
* The input pointer json cannot point to a read-only address area, such as a string constant,
* but should point to a readable and writable address area. */
CJSON_PUBLIC(void) cJSON_Minify(char *json);
/* Helper functions for creating and adding items to an object at the same time.
* They return the added item or NULL on failure. */
CJSON_PUBLIC(cJSON*) cJSON_AddNullToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddTrueToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddFalseToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddBoolToObject(cJSON * const object, const char * const name, const cJSON_bool boolean);
CJSON_PUBLIC(cJSON*) cJSON_AddNumberToObject(cJSON * const object, const char * const name, const double number);
CJSON_PUBLIC(cJSON*) cJSON_AddStringToObject(cJSON * const object, const char * const name, const char * const string);
CJSON_PUBLIC(cJSON*) cJSON_AddRawToObject(cJSON * const object, const char * const name, const char * const raw);
CJSON_PUBLIC(cJSON*) cJSON_AddObjectToObject(cJSON * const object, const char * const name);
CJSON_PUBLIC(cJSON*) cJSON_AddArrayToObject(cJSON * const object, const char * const name);
/* When assigning an integer value, it needs to be propagated to valuedouble too. */
#define cJSON_SetIntValue(object, number) ((object) ? (object)->valueint = (object)->valuedouble = (number) : (number))
/* helper for the cJSON_SetNumberValue macro */
CJSON_PUBLIC(double) cJSON_SetNumberHelper(cJSON *object, double number);
#define cJSON_SetNumberValue(object, number) ((object != NULL) ? cJSON_SetNumberHelper(object, (double)number) : (number))
/* Change the valuestring of a cJSON_String object, only takes effect when type of object is cJSON_String */
CJSON_PUBLIC(char*) cJSON_SetValuestring(cJSON *object, const char *valuestring);
/* If the object is not a boolean type this does nothing and returns cJSON_Invalid else it returns the new type*/
#define cJSON_SetBoolValue(object, boolValue) ( \
(object != NULL && ((object)->type & (cJSON_False|cJSON_True))) ? \
(object)->type=((object)->type &(~(cJSON_False|cJSON_True)))|((boolValue)?cJSON_True:cJSON_False) : \
cJSON_Invalid\
)
/* Macro for iterating over an array or object */
#define cJSON_ArrayForEach(element, array) for(element = (array != NULL) ? (array)->child : NULL; element != NULL; element = element->next)
/* malloc/free objects using the malloc/free functions that have been set with cJSON_InitHooks */
CJSON_PUBLIC(void *) cJSON_malloc(size_t size);
CJSON_PUBLIC(void) cJSON_free(void *object);
#ifdef __cplusplus
}
#endif
#endif

23
src/src/convert.cpp
Unescape View File

@ -0,0 +1,23 @@
#include <math.h>
#define EARTH_RADIUS 6378137 // 地球半径
void geodetic2enu(double latitude, double longitude, double latitude0, double longitude0, int *x, int *y) {
double flattening = 1 / 298.257223563; // 扁率
double e_square = flattening * (2.0 - flattening);
double N0 = EARTH_RADIUS / sqrt(1 - e_square * sin(latitude0) * sin(latitude0));
double X0 = N0 * cos(latitude0) * cos(longitude0);
double Y0 = N0 * cos(latitude0) * sin(longitude0);
double N = EARTH_RADIUS / sqrt(1 - e_square * sin(latitude) * sin(latitude));
double X = N * cos(latitude) * cos(longitude);
double Y = N * cos(latitude) * sin(longitude);
double deltaX = X - X0;
double deltaY = Y - Y0;
// 将浮点类型的坐标转换为整数类型
*x = (int)deltaY;
*y = (int)(-deltaX);
}

1
src/src/convert.h
Unescape View File

@ -0,0 +1 @@
void geodetic2enu(double latitude, double longitude, double latitude0, double longitude0, int *x, int *y);

342
src/src/hold.cpp
Unescape View File

@ -0,0 +1,342 @@
#include <ros/ros.h>
#include <iostream>
#include <pthread.h>
#include <stdlib.h>
#include <prometheus_msgs/ControlCommand.h>
#include <prometheus_msgs/DroneState.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <unistd.h>
#include <cstring>
#include <string>
#include <stdexcept>
#include <locale>
#include <codecvt>
#include <unicode/utypes.h>
#include <unicode/unistr.h>
#include <unicode/ustream.h>
#include "convert.h"
#include <time.h>
#include "cJSON.h"
#define NODE_NAME "terminal_control"
#define latitude_home 28.194
#define longitude_home 112.982
using namespace std;
prometheus_msgs::ControlCommand Command_to_offboard;
prometheus_msgs::ControlCommand Command_to_Move;
prometheus_msgs::ControlCommand Command_to_Return;
prometheus_msgs::ControlCommand Command_to_Return_Land;
prometheus_msgs::ControlCommand Command_to_Hold;
prometheus_msgs::ControlCommand Command_to_Takeoff;
prometheus_msgs::DroneState _DroneState;
float latitude;
float longitude;
int hours;
int minutes;
int seconds;
int arrival_flag = 0;
int hold_flag = 0;
int goal_x;
int goal_y;
int goal_z;
void Command_cb(const prometheus_msgs::ControlCommand::ConstPtr& msg){
Command_to_offboard.Command_ID = msg->Command_ID+1;
Command_to_Move.Command_ID = msg->Command_ID+1;
Command_to_Return.Command_ID = msg->Command_ID+1;
Command_to_Return_Land.Command_ID = msg->Command_ID+1;
Command_to_Hold.Command_ID = msg->Command_ID+1;
Command_to_Takeoff.Command_ID = msg->Command_ID+1;
ROS_INFO("%d",Command_to_offboard.Command_ID);
}
void Arrival_check(const prometheus_msgs::DroneState::ConstPtr& msg){
//ROS_INFO("??? go this way?");
_DroneState = *msg;
float distance = sqrt(pow(_DroneState.position[0] - Command_to_Move.Reference_State.position_ref[0], 2) +
pow(_DroneState.position[1] - Command_to_Move.Reference_State.position_ref[1], 2) +
pow(_DroneState.position[2] - Command_to_Move.Reference_State.position_ref[2], 2));
//ROS_INFO("Distance to target: %.2f", distance);
if((int)distance < 2){
arrival_flag = 1;
//ROS_INFO("Arrival_flag:%d",arrival_flag);
}
}
class SocketClient {
private:
struct sockaddr_in server_addr;
public:
int sock;
SocketClient(const char* ip_address, int port) {
// 创建 socket
sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock == -1) {
throw runtime_error("Failed to create socket!");
}
// 初始化服务器地址
memset(&server_addr, 0, sizeof(server_addr));
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(port);
server_addr.sin_addr.s_addr = inet_addr(ip_address);
// 连接服务器
if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) == -1) {
close(sock);
throw runtime_error("Failed to connect to server!");
}
}
~SocketClient() {
close(sock);
}
string receive(int buffer_size = 1024) {
char buffer[buffer_size];
int bytes_received = recv(sock, buffer, sizeof(buffer), 0);
if (bytes_received == -1) {
ROS_INFO("Failed to receive data from server!");
} else {
buffer[bytes_received] = '\0';
return string(buffer);
}
}
void send(const string& message) {
if (::send(sock, message.c_str(), message.length(), 0) == -1) {
throw runtime_error("Failed to send data to server!");
}
}
};
SocketClient client("192.168.8.100", 9090);
void *TCP_recv(void *arg){
while(true){
std::vector<char> buffer(1024);
int n = recv(client.sock, buffer.data(), buffer.size(), 0);
std::string data;
if (n > 0) {
std::string str(buffer.data(), n); // 使用 UTF-8 解码字符串
std::cout << str << '\n';
}
ROS_INFO("%s",buffer.data());
cJSON *root = cJSON_Parse(buffer.data());
const char *type= cJSON_GetObjectItem(root, "type")->valuestring;
if(!strcmp(type,"Alarming")){
const char *fire_flag = cJSON_GetObjectItem(root, "fire_flag")->valuestring;
ROS_INFO("%s",fire_flag);
if(!strcmp(fire_flag,"fire")){
hold_flag = 1;}
else
continue;
}
if(!strcmp(type,"Set")){
cJSON *latitude_r = cJSON_GetObjectItem(root, "latitude");
latitude = (float)cJSON_GetNumberValue(latitude_r);
cJSON *longitude_r = cJSON_GetObjectItem(root, "longitude");
longitude = (float)cJSON_GetNumberValue(longitude_r);
hours = cJSON_GetObjectItem(root, "hours")->valueint;
minutes = cJSON_GetObjectItem(root, "minutes")->valueint;
seconds = cJSON_GetObjectItem(root, "seconds")->valueint;
//x = cJSON_GetObjectItem(root,"x")->valueint;
//y = cJSON_GetObjectItem(root,"y")->valueint;
//z = cJSON_GetObjectItem(root,"z")->valueint;
if(latitude != 0 && longitude != 0){
//ROS_INFO("%f,%f,%f,%d,%d,%d",latitude,longitude,hours,minutes,seconds);
geodetic2enu(latitude,longitude,latitude_home, longitude_home,&goal_x,&goal_y);
ROS_INFO("x:%d,y:%d",goal_x,goal_y);
}
else{
//goal_x = x;
//goal_y = y;
//goal_z = z;
}
} //Set Check
}//while
}
int main(int argc, char** argv)
{
// 初始化 ROS 节点
ros::init(argc, argv, "control_publisher");
ros::NodeHandle nh;
ros::Rate loop_rate(10); // 发布频率为 10 Hz
ros::Subscriber Command_sub = nh.subscribe<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10, Command_cb);
ros::Subscriber position_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, Arrival_check);
ROS_INFO("I am subing");
ros::Publisher control_offboard = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);
// 创建一个发布者对象，发布 prometheus_msgs::ControlCommand 类型的消息到 /prometheus/control_command 话题
//offboard
Command_to_offboard.Mode = prometheus_msgs::ControlCommand::Idle;
Command_to_offboard.source = NODE_NAME;
Command_to_offboard.Command_ID = 1;
Command_to_offboard.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_offboard.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_offboard.Reference_State.position_ref[0] = 0;
Command_to_offboard.Reference_State.position_ref[1] = 0;
Command_to_offboard.Reference_State.position_ref[2] = 0;
Command_to_offboard.Reference_State.velocity_ref[0] = 0;
Command_to_offboard.Reference_State.velocity_ref[1] = 0;
Command_to_offboard.Reference_State.velocity_ref[2] = 0;
Command_to_offboard.Reference_State.acceleration_ref[0] = 0;
Command_to_offboard.Reference_State.acceleration_ref[1] = 0;
Command_to_offboard.Reference_State.acceleration_ref[2] = 0;
Command_to_offboard.Reference_State.yaw_ref = 999.0;
//Takeoff
Command_to_Takeoff.Mode = prometheus_msgs::ControlCommand::Move;
Command_to_Takeoff.source = NODE_NAME;
Command_to_Takeoff.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_Takeoff.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_Takeoff.Reference_State.position_ref[0] = 0;//goal_x;
Command_to_Takeoff.Reference_State.position_ref[1] = 0;//goal_y;
Command_to_Takeoff.Reference_State.position_ref[2] = 3;
Command_to_Takeoff.Reference_State.velocity_ref[0] = 0;
Command_to_Takeoff.Reference_State.velocity_ref[1] = 0;
Command_to_Takeoff.Reference_State.velocity_ref[2] = 0;
Command_to_Takeoff.Reference_State.acceleration_ref[0] = 0;
Command_to_Takeoff.Reference_State.acceleration_ref[1] = 0;
Command_to_Takeoff.Reference_State.acceleration_ref[2] = 0;
Command_to_Takeoff.Reference_State.yaw_ref = 0.0;
//Move
Command_to_Move.Mode = prometheus_msgs::ControlCommand::Move;
Command_to_Move.source = NODE_NAME;
Command_to_Move.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_Move.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_Move.Reference_State.position_ref[0] = 10;//goal_x;
Command_to_Move.Reference_State.position_ref[1] = 0;//goal_y;
Command_to_Move.Reference_State.position_ref[2] = 3;
Command_to_Move.Reference_State.velocity_ref[0] = 0;
Command_to_Move.Reference_State.velocity_ref[1] = 0;
Command_to_Move.Reference_State.velocity_ref[2] = 0;
Command_to_Move.Reference_State.acceleration_ref[0] = 0;
Command_to_Move.Reference_State.acceleration_ref[1] = 0;
Command_to_Move.Reference_State.acceleration_ref[2] = 0;
Command_to_Move.Reference_State.yaw_ref = 0.0;
//Arrive check and return
Command_to_Return.Mode = prometheus_msgs::ControlCommand::Move;
Command_to_Return.source = NODE_NAME;
Command_to_Return.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_Return.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_Return.Reference_State.position_ref[0] = 0;
Command_to_Return.Reference_State.position_ref[1] = 0;
Command_to_Return.Reference_State.position_ref[2] = Command_to_Move.Reference_State.position_ref[2];
Command_to_Return.Reference_State.velocity_ref[0] = 0;
Command_to_Return.Reference_State.velocity_ref[1] = 0;
Command_to_Return.Reference_State.velocity_ref[2] = 0;
Command_to_Return.Reference_State.acceleration_ref[0] = 0;
Command_to_Return.Reference_State.acceleration_ref[1] = 0;
Command_to_Return.Reference_State.acceleration_ref[2] = 0;
Command_to_Return.Reference_State.yaw_ref = 0.0;
//Land
Command_to_Return_Land.Mode = prometheus_msgs::ControlCommand::Land;
Command_to_Return_Land.source = NODE_NAME;
Command_to_Return_Land.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_Return_Land.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_Return_Land.Reference_State.velocity_ref[0] = 0;
Command_to_Return_Land.Reference_State.velocity_ref[1] = 0;
Command_to_Return_Land.Reference_State.velocity_ref[2] = 0;
Command_to_Return_Land.Reference_State.acceleration_ref[0] = 0;
Command_to_Return_Land.Reference_State.acceleration_ref[1] = 0;
Command_to_Return_Land.Reference_State.acceleration_ref[2] = 0;
Command_to_Return_Land.Reference_State.yaw_ref = 0.0;
//Hold
Command_to_Hold.Mode = prometheus_msgs::ControlCommand::Hold;
Command_to_Hold.source = NODE_NAME;
Command_to_Hold.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_POS;
Command_to_Hold.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
Command_to_Hold.Reference_State.position_ref[0] = _DroneState.position[0];
Command_to_Hold.Reference_State.position_ref[1] = _DroneState.position[1];
Command_to_Hold.Reference_State.position_ref[2] = _DroneState.position[2];
Command_to_Hold.Reference_State.velocity_ref[0] = 0;
Command_to_Hold.Reference_State.velocity_ref[1] = 0;
Command_to_Hold.Reference_State.velocity_ref[2] = 0;
Command_to_Hold.Reference_State.acceleration_ref[0] = 0;
Command_to_Hold.Reference_State.acceleration_ref[1] = 0;
Command_to_Hold.Reference_State.acceleration_ref[2] = 0;
Command_to_Hold.Reference_State.yaw_ref = 0.0;
// 循环发布消息
while (ros::ok())
{
clock_t start_time, end_time;
ros::AsyncSpinner spinner(1); // 创建一个只有一个线程的 Spinner
spinner.start();
ros::spinOnce();
//int buffer_size = 1024;
pthread_t tcp_rec;
int tcp_ret = pthread_create(&tcp_rec, NULL, TCP_recv, NULL);
// 创建一个新的 prometheus_msgs::ControlCommand 消息
//发布消息
time_t now = time(NULL);
struct tm *local = localtime(&now);
ROS_INFO("%d,%d",local->tm_hour,local->tm_min);
if(local->tm_hour == hours && local->tm_min == minutes && local->tm_sec == seconds){
control_offboard.publish(Command_to_offboard);
ROS_INFO("received");
sleep(1);
control_offboard.publish(Command_to_Takeoff);
sleep(20);
control_offboard.publish(Command_to_Move);
start_time = clock();
while(true){
end_time = clock();
double running_time = (double)(end_time - start_time) / CLOCKS_PER_SEC;
if(running_time <= 30){
if(hold_flag == 1){
control_offboard.publish(Command_to_Hold);
hold_flag = 0;
arrival_flag = 0;
loop_rate.sleep();
}
}
else
break;
}
}
//ROS_INFO("check_arrival:%d",arrival_flag);
if(arrival_flag == 1){
ROS_INFO("start return");
sleep(5);
control_offboard.publish(Command_to_Return);
ROS_INFO("arrival");
loop_rate.sleep();
loop_rate.sleep();
start_time = clock();
while(true){
end_time = clock();
double running_time = (double)(end_time - start_time) / CLOCKS_PER_SEC;
if(running_time <= 30){
if(hold_flag == 1){
control_offboard.publish(Command_to_Hold);
/*cJSON *find = cJSON_CreateObject();
cJSON_AddStringToObject(find, "latitude", 0);
cJSON_AddNumberToObject(find, "longitude", 25);
char *json_str = cJSON_Print(find);
client.send(json_str);
hold_flag = 0;*/
loop_rate.sleep();
}
}
else
break;
}
control_offboard.publish(Command_to_Return_Land);
arrival_flag = 0;
}
/*if(hold_flag ==1){
control_offboard.publish(Command_to_Hold);
loop_rate.sleep();
hold_flag = 0;
}*/
// 延时等待，以便控制发布速度
loop_rate.sleep();
}
return 0;
}
Write Preview
Loading…
Cancel
Save