Title Source: https://blog.csdn.net/weixin_40689871/article/details/108186903

http://wiki.ros.org/ROS/Tutorials

1. 2. 3. Dark blue course assignment corresponding to the first link

4. Input two parameters from the command line and design an adder in the way of server and client.

It was intended to use ide KDevelop for debug ging, but a bad bug occurred in the process of use (after restarting, it was prompted that roscore was not installed, and rosrun could not find the installed Library). The error could be repeated. It was initially suspected that it was an ros environment problem, so it was abandoned because of the trouble.

Question 1: it's almost the same as the example of dark blue video teaching. Just put them together. It's no difficulty.

#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include"turtlesim/Pose.h"

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework1");
    ros::NodeHandle n;
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);
    ros::Subscriber pos_sub = n.subscribe("/turtle1/pose",10,poseCallback);
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;
        
        turtle_vel_pub.publish(vel_msg);
		ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
vel_msg.linear.x, vel_msg.angular.z);
        ros::spinOnce();
        loop_rate.sleep();
        
    }
    
    return 0;
    
}

Question 2: no difficulty

#include<ros/ros.h>
#include <turtlesim/Spawn.h>
int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework2");
    ros::NodeHandle n;
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    srv.request.x = 5.0;
    srv.request.y = 5.0;
    srv.request.name = "turtle2";
    
    ROS_INFO("Call service to spwan turtle[x:%0.6f,y:%0.6f,name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);
    ROS_INFO("Spwan turtle successfully [name:%s]",srv.response.name.c_str());
    return 0;
    
}

Question 3:

Divide into two nodes to complete this task
The first node function is to generate a new turtle. The function of generating turtle has been developed by spawn H is implemented. This function exists as a service, so you only need to use the client to req the service.

/opt/ros/melodic/share/turtlesim/srv/Spawn. The contents of the SRV file are as follows

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

From the title, you need to enter name on the command line, and then the positions do not overlap. So req needs to enter x, y, name
The tortoise command is used to create the location of the tortoise, and then the tortoise command is used to obtain the location of all the topics in turn (assuming that the tortoise command is used to obtain the location of all the topics, and then the tortoise command is used to obtain the location of all the topics, and then the tortoise command is used to generate the coordinates of all the topics), Then I thought this method was too troublesome, so I gave up.
Then the idea is: establish a client, the tortoise name is specified by the command line, obtain a random number after obtaining the instruction, generate SRV req content, and then apply to the spawn server.

#include<ros/ros.h>
#include<turtlesim/Spawn.h>
#include"time.h"


int main(int argc,char **argv)
{
    if(argc!=2)
    {
        ROS_INFO("usage:please input turtle`s name:");
        return 1;
    }
    ros::init(argc,argv,"homework3");
    ros::NodeHandle n;
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    srand(time(NULL));
    srv.request.x = random()/100%10;
    srv.request.y = random()/100%10;
    srv.request.name = argv[1];
    
    ROS_INFO("Call service to spwan turtle[x:%0.6f,y:%0.6f,name:%s]",srv.request.x,srv.request.y,srv.request.name.c_str());
    add_turtle.call(srv);
    ROS_INFO("Spwan turtle successfully [name:%s]",srv.response.name.c_str());
    return 0;
}

The second node function is to control motion

The goal of the mission is to control the stop / start of any turtle and give speed.

Stop is equivalent to speed = 0, so the essence is to control the turtle speed.

From the previous study, we know that after creating an object, a subscriber will be automatically generated to subscribe to / < turtle name > / CMD_ Vel message, if we want to control the movement of turtles, we only need to publish a message to this end.

/opt/ros/melodic/share/geometry_ msgs/msg/Twist. The contents of the MSG file are as follows

# This expresses velocity in free space broken into its linear and angular parts.
Vector3  linear
Vector3  angular
In terms of task requirements, there are three input values: Turtle name, stop start flag, speed / angular speed

Establish the publishing end, obtain the command from the command line, and then issue the command

For how to input control parameters, python implementation is very simple. You can use argparse to realize optional parameters on the command line. It is too difficult to input optional parameters on the command line in c + +, so you can read the parameter file directly. Subsequently, you can use roslaunch to read the server parameters.

Use jsoncpp to read the parameter file and publish it (if you stop, just send the default parameters (all 0).

#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<fstream>
#include<jsoncpp/json/json.h>
//Input parameter turtle name start stop bit parameter file directory
//json file example.
/*
{
    "linear":
    {
        "x":1.0,
        "y":2.0,
        "z":0.0
    },
    "angular":
    {
        "x":0.0,
        "y":0.0,
        "z":5.0
    }
}
*/
geometry_msgs::Twist LoadParameters(std::string file_name)
{

    std::ifstream file;
    file.open(file_name,std::ios::binary);
    
    if(!file.is_open())
    {
        std::cout<<"open json file failed"<<std::endl;
    }
    
    Json::Reader reader;
    Json::Value root;
    
    geometry_msgs::Twist vel_msg;
    if(reader.parse(file,root))
    {
        vel_msg.linear.x = root["linear"]["x"].asDouble();
        vel_msg.linear.y = root["linear"]["y"].asDouble();
        vel_msg.linear.z = root["linear"]["y"].asDouble();
        vel_msg.angular.x = root["angular"]["x"].asDouble();
        vel_msg.angular.y = root["angular"]["y"].asDouble();
        vel_msg.angular.z = root["angular"]["z"].asDouble();        
//         std::cout << "Reading Complete!" << std::endl;

    }
    
    file.close();
    return vel_msg;
}

int main(int argc,char **argv)
{
    ros::init(argc,argv,"homework3_2");
    ros::NodeHandle n;
    if(argc <= 3 && argv[2] != 0)
    {
        ROS_INFO("usage:please input turtle control command");
        return -1;
        
    }
    std::string name = argv[1];
    name.insert(0,"/");
    name.append("/cmd_vel");
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>(name,10);
    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        geometry_msgs::Twist vel_msg;
        if (argv[2] != 0)
        {
            vel_msg = LoadParameters(argv[3]);
        }
        turtle_vel_pub.publish(vel_msg);
        loop_rate.sleep();
        
    }
    return 0;   
}