About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Camera + Dynamixel Controller

Description: This tutorial explains how to use the dynamixel motor in position mode + a camera que take pictures using a client service. Here we're using the two previous topics together.

Tutorial Level: INTERMEDIATE

Creating a Picture Taker Package

The following command creates a 'picture_taker' package. This package is dependent on the 'message_generation', 'std_msgs', 'roscpp', 'cv_bridge' and 'image_transport' packages.

$ cd ~/catkin_ws/src
$ catkin_create_pkg picture_taker message_generation std_msgs roscpp  image_transport

Modifying the Package Configuration (package.xml)

The ‘package.xml’ file, one of the required ROS configuration files, is an XML file containing the package information such as the package name, author, license, and dependent packages. Let’s open the file using an editor (such as gedit, vim, emacs, etc.) with the following command and modify it for the current node.

$ cd
$ gedit package.xml

The following code shows how to modify the 'package.xml' file to match package you created.

<?xml version="1.0"?>
<package>
<name>picture_taker</name>
<version>0.1.0</version>
<description>ROS tutorial package to learn the topic</description>
<license>Apache License 2.0</license>
<author email="pyo@robotis.com">Yoonseok Pyo</author>
<maintainer email="pyo@robotis.com">Yoonseok Pyo</maintainer>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ros_tutorials/issues</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ros_tutorials.git</url>
<url type="website">http://www.robotis.com</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>image_transport</run_depend>
<export></export>
</package>

Modifying the Build Configuration File (CMakelist.txt)

Catkin, which is the build system of ROS, uses CMake. The build environment is described in the 'CMakelist.txt' file in the package folder. This file configures executable file creating, dependency package priority build, link creation, and so on.

$ gedit CMakelist.txt

The following is the modified code of CMakeLists.txt for the package we created.

cmake_minimum_required(VERSION 2.8.3)
project(picture_taker)
## A component package required when building the Catkin.
## Has dependency on message_generation, std_msgs, roscpp.
## An error occurs during the build if these packages do not exist.
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs roscpp sensor_msgs image_transport)
find_package(OpenCV REQUIRED)
## Declaration Message: MsgTutorial.msg
add_message_files(FILES MsgDynamixel.msg)
add_service_files(FILES image_cmd.srv)
## an option to configure the dependent message.
## An error occurs duing the build if "std_msgs" is not installed.
generate_messages(DEPENDENCIES std_msgs)
## A Catkin package option that describes the library, the Catkin build dependencies,
## and the system dependent packages.
catkin_package(
LIBRARIES picture_taker
CATKIN_DEPENDS std_msgs roscpp
)
## Include directory configuration.
include_directories(${catkin_INCLUDE_DIRS})
## Build option for the "topic_publisher" node.
## Configuration of Executable files, target link libraries, and additional dependencies.
add_executable(motor_controller src/motor_controller.cpp)
add_dependencies(motor_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(motor_controller ${catkin_LIBRARIES})

add_executable(cam_publisher src/cam_publisher.cpp)
target_link_libraries(cam_publisher ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_dependencies(cam_publisher picture_taker_generate_messages_cpp)

add_executable(picture_server src/picture_server.cpp)
target_link_libraries(picture_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
add_dependencies(picture_server picture_taker_generate_messages_cpp)

Writing the Message File

The following option is added to the CMakeLists.txt file.

add_message_files(FILES MsgDynamixel.msg)

The above option indicates to include the message file 'MsgDynamixel.msg', which will be used in this example node, when building the package. Let's create the file in the following order:

$ roscd picture_taker
$ mkdir msg
$ cd msg
$ gedit MsgDynamixel.msg

The content in the message file is simple:

float64 data

Writing the Controller Node

That is, the ‘motor_controller.cpp’ file is built in the src’ folder to create the ‘motor_controller’ executable file. Let’s create a code that performs publisher and subscriber nodes functions in the following order:

$ roscd picture_taker/src
$ gedit motor_controller.cpp

The following code should be put in motor_controller.cp, change the line /home/everton/robot for your folder.

// ROS Default Header File
#include "ros/ros.h"
// MsgTutorial Message File Header
// The header file is automatically created when building the package.

#include "dynamixel_position_control/MsgDynamixel.h"
#include "dynamixel_msgs/JointState.h"
#include "picture_taker/image_cmd.h"
#include <sstream>

#define GOAL_POS 0
#define CURRENT_POS 1
#define ERROR 2
#define LOAD 3
#define ERROR_POS 0.01
#define ERROR_NEG -0.01
#define TX 0
#define RX 1

void motor_command(ros::Publisher dynamixel_publisher, picture_taker::image_cmd service, ros::ServiceClient service_client); //Receive the current motor position and send the next position
bool motor_init(float qtd_pos); //Initialize motor variables

struct Motor{
 float motor_state[4], count, pos;
 int Estado;
 bool moving;
 dynamixel_position_control::MsgDynamixel msg;
}MX28;

//Initialize motor variables
bool motor_init(float qtd_pos)
{
  if(qtd_pos > 360 || qtd_pos <= 0){
   ROS_ERROR("Qtd_pos value should be between 0 and 360");
   return false;
  }
  MX28.Estado = TX;
  MX28.count = 0;
  MX28.pos = 6.14/qtd_pos;
  for(int i = 0; i < 4; i++) MX28.motor_state[i] = 0xff;
  MX28.moving = false;
  MX28.msg.data = 0;
  return true;
}

// Message callback function. This is a function is called when a topic
// message named 'tilt_controller/state' is received.
void msgCallback(const dynamixel_msgs::JointState::ConstPtr& msg)
{
 MX28.motor_state[GOAL_POS] = msg->goal_pos;
 MX28.motor_state[CURRENT_POS] = msg->current_pos;
 MX28.motor_state[ERROR] = msg->error;
 MX28.motor_state[LOAD] = msg->load;
 MX28.moving = msg->is_moving;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "dynamixel_publisher"); // Initializes Node Name
  ros::init(argc, argv, "dynamixel_subscriber");
  ros::NodeHandle nh; // Node handle declaration for communication with ROS system

  float qtd_pos;
  nh.param("Qtd_Pos", qtd_pos, qtd_pos); //Receive qtd_pos from launch file (value should be between 0 and 360)

  if(!motor_init(qtd_pos)) return 0;

  // Declare publisher, create publisher 'dynamixel_publisher' using the 'MsgDynamixel'
  // message file from the 'dynamixel_control_position' package. The topic name is
  // 'tilt_controller/command' and the size of the publisher queue is set to 100
   ros::Publisher dynamixel_publisher = nh.advertise<dynamixel_position_control::MsgDynamixel>("tilt_controller/command", 100);

  // Declares subscriber. Create subscriber 'dynamixel_subscriber' using the 'MsgDynamixel'
  // message file from the 'dynamixel_control_position' package. The topic name is
  // 'tilt_controller/state' and the size of the subscribe queue is set to 100.
  ros::Subscriber dynamixel_subscriber = nh.subscribe("tilt_controller/state", 100, msgCallback);
  ros::ServiceClient service_client = nh.serviceClient<picture_taker::image_cmd>("image_cmd");

  picture_taker::image_cmd service;
  service.request.cmd = true;
  service.request.path = "/home/everton/robot/";

  ros::Rate loop_rate(5); // Set the loop period (Hz)


   while (ros::ok()){
    // Goes to sleep according to the loop rate defined above.
    loop_rate.sleep();

    // A function for calling a callback function, waiting for a message to be
    // received, and executing a callback function when it is received
    ros::spinOnce();
    motor_command(dynamixel_publisher, service, service_client);
   }

   return 0;
}

//Receive the current motor position and send the next position
void motor_command(ros::Publisher dynamixel_publisher, picture_taker::image_cmd service, ros::ServiceClient service_client)
{
  switch(MX28.Estado)
  {
   case TX:
    MX28.msg.data = MX28.count;
    dynamixel_publisher.publish(MX28.msg); // Publishes 'MX28.msg' message
    ROS_INFO("Send Position = %f", MX28.msg.data);
    MX28.Estado = RX;
   break;

   case RX:
    if((MX28.count - MX28.motor_state[CURRENT_POS] <= ERROR_POS) && (MX28.count - MX28.motor_state[CURRENT_POS] >= ERROR_NEG))
    {
     ROS_INFO("Current Position = %f", MX28.motor_state[CURRENT_POS]);
     std::ostringstream count_string;
     count_string << MX28.count;
     service.request.num_name = count_string.str();
     service_client.call(service);
     if (service.response.result == 1)
     {
       MX28.count += MX28.pos;
       if(MX28.count >= 6.14) MX28.count = 0;
       MX28.Estado = TX;
     }else MX28.Estado = RX;
    }else MX28.Estado = TX;
   break;
  }
}

The 'cam_publisher.cpp’ file is built in the src’ folder to create the ‘cam_publisher’ executable file.

$ roscd picture_taker/src
$ gedit cam_publisher.cpp

The following code should be put in cam_publisher.cpp

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream> // for converting the command line parameter to integer

int main(int argc, char** argv){
  // Check if video source has been passed as a parameter
  if(argv[1] == NULL) return 1;

  ros::init(argc, argv, "image_publisher");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Publisher pub = it.advertise("cam_pub", 1);
  int cam_device;
  //condition to verify if the camera device was passed as the first terminal parameter "argv[1]" or as the "cam_device" parameter in the .launch
 //id 0 device is passed as default
  if (argc < 2)
        nh.param("cam_device", cam_device, 0);
  else{
          // Convert the passed as command line parameter index for the video device to an integer
          std::istringstream cam_deviceCmd(argv[1]);
          // Check if it is indeed a number
          if(!(cam_deviceCmd >> cam_device)) return 1;
  }
  //initialize a VideoCapture variable to get camera data
  cv::VideoCapture cap(cam_device);
  // Check if video device can be opened with the given index
  if(!cap.isOpened()) return 1;
  cv::Mat frame;
  sensor_msgs::ImagePtr msg;

  int image_show;
  nh.param("image_show", image_show, 0);

  while (nh.ok()) {
        cap >> frame;
        //show the image
        if (image_show){
                cv::imshow("Cam View", frame);
                cv::waitKey(30);
        }
        // Check if grabbed frame is actually full with some content, then publish the image
        if(!frame.empty()) {
                msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
                pub.publish(msg);
        }
  }
}

The 'picture_server.cpp’ file is built in the src’ folder to create the ‘picture_server.cpp’ executable file.

$ roscd picture_taker/src
$ gedit picture_server.cpp

The following code should be put in picture_server.cpp

#include <string>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include "picture_taker/image_cmd.h"
#include <sstream>
//Class creation to allow the use of camera callback msg in the service
class PictureServer{
        cv::Mat picture;

public:
        //callback to get camera data through "image_pub" topic
        void imageCallback(const sensor_msgs::ImageConstPtr& msg){
          try{
                picture = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
          }
          catch (cv_bridge::Exception& e){
                ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
          }
        }
        // service callback that receives "angle" (int representing image name), "path" (path to save image data) and "cmd" (comand confirming if the camera data should be saved). The service response should return a "result" returning 1 if the data was correctly saved
        bool check_and_print(picture_taker::image_cmd::Request &req, picture_taker::image_cmd::Response &res){
                if (req.cmd){
                        //image name composed by path (finished with "/")+ capture angle+extension
                        std::string im_name = req.path + req.num_name+ ".png";
                        //checking if the picture has a valid content, otherwise system would failed and stop trying to write the image
                        if(!picture.empty()){
                                if (!cv::imwrite (im_name, picture)){
                                        res.result = 0;
                                        std::cout<<"Image can not be saved as '"<<im_name<<"'\n";
                                }else{
                                        std::cout<<"Image saved in '"<<im_name<<"'\n";
                                        res.result = 1; // represent success to save the image
                                        }
                        }else{
                                res.result = 0;
                                ROS_ERROR("Failed to save image\n"); // represent fail to save the image
                        }
                }else{
                        ROS_INFO("Returning without saving image\n");
                        res.result = 2;// represent that server was called, but image was not requested
                }
        }
};


int main(int argc, char **argv)
{
  PictureServer mi;
  ros::init(argc, argv, "Img_Ctrl_server");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  ros::ServiceServer service = nh.advertiseService("image_cmd", &PictureServer::check_and_print, &mi);
  image_transport::Subscriber sub = it.subscribe("cam_pub", 1, &PictureServer::imageCallback, &mi);

  ros::spin();
}

Before build this package you need to take out the two previous packages from the folder catkin_ws (Camera Picture Server Tutorial and Dynamixel Position Control) because nodes with same names cause conflicts.

Let's build it with catkin_make.

$ cd ~/catkin_ws
$ catkin_make

Writing the Launch File

Roslaunch is a command that executes more than one node in the specified package or sets execution options.

The following command creates the launch file.

$ roscd picture_taker
$ mkdir launch
$ cd launch
$ gedit controller.launch

The following code should be put in controller.launch

<!-- -*- mode: XML -*- -->

<launch>
    <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
        <rosparam>
            namespace: dxl_manager
            serial_ports:
                pan_tilt_port:
                    port_name: "/dev/ttyUSB0"
                    baud_rate: 57600
                    min_motor_id: 1
                    max_motor_id: 25
                    update_rate: 20
        </rosparam>
    </node>
    <!-- Start tilt joint controller -->
    <rosparam file="$(find picture_taker)config/multi_turn.yaml" command="load"/>
    <node name="tilt_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                tilt_controller"
          output="screen"/>
   <param name="Qtd_Pos" value="16" type="int"/>
   <node pkg="picture_taker" type="motor_controller" name="motor_controller" output="screen"/>
   <param name="cam_device" value="1" type="int"/>
   <param name="image_show" value="1" type="int"/>
   <node pkg="picture_taker" name="cam_publisher" type="cam_publisher"/>
   <node pkg="picture_taker" name="picture_server" type="picture_server"/>
</launch>

The file multi_turn.yaml is your configure file, so let's create it.

$ roscd picture_taker
$ mkdir config
$ cd config
$ gedit gedit multi_turn.yaml

The following code should be put in your multi_turn.yaml file.

tilt_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: tilt_joint
    joint_speed: 4
    motor:
        id: 1
        init: 0
        min: 0
        max: 49152

Running the Launch File

Open the terminal and type:

roslaunch dynamixel_position_control controller.launch

If all is right, the motor must move between 0 and 2PI in 16 steps taking pictures and saving it in directory /home/username/robot. The parameter Qtd_Pos in launch file is the number of photos that will taken, it should be between 0 and 360.