canopen_ros_common.cpp

Go to the documentation of this file.

//ROS typedefs
#include "ros/ros.h"
#include <control_msgs/JointTrajectoryControllerState.h>
#include <sensor_msgs/JointState.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <std_msgs/String.h>
#include <brics_actuator/JointVelocities.h>
#include <cob_srvs/Trigger.h>
#include <cob_srvs/Trigger.h>

/* protected region user include files on begin */
#include <ipa_canopen_core/canopen.h>
#include <ipa_canopen_ros/JointLimits.h>
#include <urdf/model.h>
/* protected region user include files end */

class canopen_ros_config
{
public:
                std::string can_device;
                std::string can_baudrate;
                std::string modul_ids;
                std::string joint_names;
                std::string robot_description;

};

class canopen_ros_data
{
// autogenerated: don't touch this class
public:
//input data
        brics_actuator::JointVelocities in_command_vel;
  
        
//output data
        control_msgs::JointTrajectoryControllerState out_state;
        sensor_msgs::JointState out_joint_states;
        diagnostic_msgs::DiagnosticArray out_diagnostics;
        std_msgs::String out_current_operationmode;
 

};

class canopen_ros_impl
{
        /* protected region user member variables on begin */
        bool is_initialized_;
        std::string chain_name_; // FIXME: get from parameter server
        int dof_;
        std::vector<std::string> joint_names_; // HACK: does not work yet because BRIDE doesn't support list parameters
        std::vector<uint8_t> modul_ids_; // HACK: does not work yet because BRIDE doesn't support list parameters
        JointLimits* joint_limits_;
        /* protected region user member variables end */

public:
    canopen_ros_impl() 
    {
        /* protected region user constructor on begin */
        is_initialized_ = false;

        chain_name_ = "arm_controller"; // FIXME: get from parameter server
        joint_names_.push_back("arm_1_joint"); // HACK: does not work yet because BRIDE doesn't support list parameters
        //joint_names_.push_back("arm_2_joint"); // HACK: does not work yet because BRIDE doesn't support list parameters
        //joint_names_.push_back("arm_3_joint"); // HACK: does not work yet because BRIDE doesn't support list parameters
        modul_ids_.push_back(uint16_t(1)); // HACK: does not work yet because BRIDE doesn't support list parameters

        joint_limits_ = new JointLimits();
                /* protected region user constructor end */
    }
    void configure(canopen_ros_config config) 
    {
        /* protected region user configure on begin */
        // get dof from joint_names vector
        std::cout << "joint_names: " << config.joint_names << std::endl;
        // dof_ = config.joint_names.size();
        dof_ = joint_names_.size(); // HACK: does not work yet because BRIDE doesn't support list parameters
        ROS_INFO("DOF: %d", dof_);
        joint_limits_->setDOF(dof_);

        if (config.robot_description.size() == 0)
        {
                ROS_ERROR("Unable to load robot model from parameter robot_description, shutting down node...");
                exit(EXIT_FAILURE);
        }

                urdf::Model model;
                if (!model.initString(config.robot_description))
                {
                        ROS_ERROR("Failed to parse urdf file, shutting down node...");
                        exit(EXIT_FAILURE);
                }
                ROS_INFO("Successfully parsed urdf file");

                std::vector<double> MaxVelocities(dof_);
                for (int i = 0; i < dof_; i++)
                {
                        //MaxVelocities[i] = model.getJoint(config.joint_names[i].c_str())->limits->velocity;
                        MaxVelocities[i] = model.getJoint(joint_names_[i].c_str())->limits->velocity; // HACK: does not work yet because BRIDE doesn't support list parameters
                        std::cout << "MaxVelocities[" << joint_names_[i].c_str() <<"]: " << MaxVelocities[i] << std::endl;
                }
                joint_limits_->setMaxVelocities(MaxVelocities);

                std::vector<double> LowerLimits(dof_);
                for (int i = 0; i < dof_; i++)
                {
                        //LowerLimits[i] = model.getJoint(config.joint_names[i].c_str())->limits->lower;
                        LowerLimits[i] = model.getJoint(joint_names_[i].c_str())->limits->lower; // HACK: does not work yet because BRIDE doesn't support list parameters
                        std::cout << "LowerLimits[" << joint_names_[i].c_str() <<"]: " << LowerLimits[i] << std::endl;
                }
                joint_limits_->setLowerLimits(LowerLimits);

                // Get upper limits out of urdf model
                std::vector<double> UpperLimits(dof_);
                for (int i = 0; i < dof_; i++)
                {
                        //UpperLimits[i] = model.getJoint(config.joint_names[i][i].c_str())->limits->upper;
                        UpperLimits[i] = model.getJoint(joint_names_[i].c_str())->limits->upper; // HACK: does not work yet because BRIDE doesn't support list parameters
                        std::cout << "UpperLimits[" << joint_names_[i].c_str() <<"]: " << UpperLimits[i] << std::endl;
                }
                joint_limits_->setUpperLimits(UpperLimits);

                std::vector<double> Offsets(dof_);
                for (int i = 0; i < dof_; i++)
                {
                        //Offsets[i] = model.getJoint(config.joint_names[i][i].c_str())->calibration->rising.get()[0];
                        Offsets[i] = model.getJoint(joint_names_[i].c_str())->calibration->rising.get()[0]; // HACK: does not work yet because BRIDE doesn't support list parameters
                        std::cout << "Offsets[" << joint_names_[i].c_str() <<"]: " << Offsets[i] << std::endl;
                }
                joint_limits_->setOffsets(Offsets);

                // create device groups
        std::vector<std::string> devices;
        devices.push_back(static_cast<std::string>(config.can_device));

        for (unsigned int i=0; i<dof_; i++)
        {
                canopen::devices[ modul_ids_[i] ] = canopen::Device(modul_ids_[i], joint_names_[i], chain_name_, devices[i]);
        }

        canopen::deviceGroups[ chain_name_ ] = canopen::DeviceGroup(modul_ids_, joint_names_);

        /* protected region user configure end */
    }
    void update(canopen_ros_data &data, canopen_ros_config config)
    {
        /* protected region user update on begin */
        //ROS_WARN("update start");
        if (is_initialized_)
        {
                //TODO:
                //send new vel to canopen
                //std::cout << data.in_command_vel << std::endl;
                //move(data.in_command_vel)

                std::vector<double> velocities;
                for (int i=0; i<data.in_command_vel.velocities.size(); i++ )
                {
                        velocities.push_back(data.in_command_vel.velocities[i].value);
                }

                canopen::deviceGroups[chain_name_].setVel(velocities);



                int counter = 0;
                        std::vector <double> positions;
                        std::vector <double> desired_positions;

                        for (auto device : canopen::devices)
                        {
                                double pos = (double)device.second.getActualPos() + joint_limits_->getOffsets()[counter];
                                double des_pos = (double)device.second.getDesiredPos() + joint_limits_->getOffsets()[counter];
                                positions.push_back(pos);
                                desired_positions.push_back(des_pos);
                                counter++;
                        }

                        for (auto dg : (canopen::deviceGroups))
                        {
                                // joint state
                                data.out_joint_states.name = dg.second.getNames();
                                data.out_joint_states.header.stamp = ros::Time::now(); // todo: possibly better use timestamp of hardware msg?

                                data.out_joint_states.position = positions;
                                data.out_joint_states.velocity = dg.second.getActualVel();
                                data.out_joint_states.effort = std::vector<double>(dg.second.getNames().size(), 0.0);

                                // controller state
                                data.out_state.header.stamp = data.out_joint_states.header.stamp;
                                data.out_state.actual.positions = data.out_joint_states.position;
                                data.out_state.actual.velocities = data.out_joint_states.velocity;
                                data.out_state.desired.positions = desired_positions;//dg.second.getDesiredPos();
                                data.out_state.desired.velocities = dg.second.getDesiredVel();

                                // operation mode
                                data.out_current_operationmode.data = "velocity";
                        }

                //get current joint states
                //joint_pos = ...
                //joint_vel = ...

                //set current joint_states to out_joint_state and out_state
                //data.out_joint_state =
                //data.out_state =

                //get diagnostics info from canopen
                diagnostic_msgs::DiagnosticStatus diagnostic_status;
                //diagnostic_status = ...

                //set diagostics to out_diagnostics
                data.out_diagnostics.status.push_back(diagnostic_status);
        }
                /* protected region user update end */
    }

        bool callback_init(cob_srvs::Trigger::Request  &req, cob_srvs::Trigger::Response &res , canopen_ros_config config)
        {
                /* protected region user implementation of service callback for init on begin */
                ROS_INFO("Initializing canopen...");
                if (!is_initialized_)
                {
                        // open device
                        ROS_WARN("open device");
                        is_initialized_ = canopen::openConnection(config.can_device, config.can_baudrate); // TODO: add feature for different baudrates, // TODO: check for return value


                        // default PDO mapping
                        ROS_WARN("default PDO mapping");
                        canopen::sendPos = canopen::defaultPDOOutgoing_interpolated;
                    for (auto it : canopen::devices) {
                        canopen::incomingPDOHandlers[ 0x180 + it.first ] = [it](const TPCANRdMsg mS) { canopen::defaultPDO_incoming_status( it.first, mS ); };
                                canopen::incomingPDOHandlers[ 0x480 + it.first ] = [it](const TPCANRdMsg mP) { canopen::defaultPDO_incoming_pos( it.first, mP ); };
                        canopen::incomingEMCYHandlers[ 0x081 + it.first ] = [it](const TPCANRdMsg mE) { canopen::defaultEMCY_incoming( it.first, mE ); };
                    }

                    // init device
                    ROS_WARN("init device");
                    canopen::init(config.can_device, canopen::syncInterval); // TODO: check for return value
                    std::this_thread::sleep_for(std::chrono::milliseconds(100));

                    // init modules
                    ROS_WARN("init modules");
                    for (auto device : canopen::devices)
                    {
                        canopen::sendSDO(device.second.getCANid(), canopen::MODES_OF_OPERATION, canopen::MODES_OF_OPERATION_INTERPOLATED_POSITION_MODE); // TODO: check for return value
                        std::cout << "Setting IP mode for: " << (uint16_t)device.second.getCANid() << std::endl;
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    }

                    std::this_thread::sleep_for(std::chrono::milliseconds(100));

                    // init device thread
                    ROS_WARN("init device thread");
                    canopen::initDeviceManagerThread(canopen::deviceManager); // TODO: check for return value

                        if (is_initialized_)
                        {
                           res.success.data = true;
                           res.error_message.data = "initializing canopen successful";
                          ROS_INFO("... initializing canopen successful");
                        }
                        else
                        {
                          res.success.data = false;
                          res.error_message.data = "dummy error message"; // TODO get error message from canopen
                          ROS_ERROR("...initializing canopen not successful. error: %s", res.error_message.data.c_str());
                        }
                }
                else
                {
                        res.success.data = true;
                        res.error_message.data = "canopen already initialized, please user recover";
                        ROS_WARN("...initializing canopen not successful. error: %s",res.error_message.data.c_str());
                }

                /* protected region user implementation of service callback for init end */
                return true;
        }
        bool callback_recover(cob_srvs::Trigger::Request  &req, cob_srvs::Trigger::Response &res , canopen_ros_config config)
        {
                /* protected region user implementation of service callback for recover on begin */
                ROS_INFO("Recovering canopen...");
                if (is_initialized_)
                {
                        bool is_recovered = true; // TODO: fill in canopen command
                        canopen::recover(config.can_device, canopen::syncInterval);
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));

                        for (auto device : canopen::devices)
                        {
                                canopen::sendSDO(device.second.getCANid(), canopen::MODES_OF_OPERATION, canopen::MODES_OF_OPERATION_INTERPOLATED_POSITION_MODE);
                                std::cout << "Setting IP mode for: " << (uint16_t)device.second.getCANid() << std::endl;
                                std::this_thread::sleep_for(std::chrono::milliseconds(100));

                                canopen::devices[device.second.getCANid()].setDesiredPos((double)device.second.getActualPos());
                                canopen::devices[device.second.getCANid()].setDesiredVel(0);

                                canopen::sendPos((uint16_t)device.second.getCANid(), (double)device.second.getDesiredPos());
                                canopen::sendPos((uint16_t)device.second.getCANid(), (double)device.second.getDesiredPos());

                        }


                        if (is_recovered)
                        {
                                res.success.data = true;
                                res.error_message.data = "recovering canopen successful";
                                ROS_INFO("...recovering canopen successful.");
                        }
                        else
                        {
                                res.success.data = false;
                                res.error_message.data = "dummy error message"; // TODO get error message from canopen;
                                ROS_ERROR("...recovering canopen not successful. error: %s", res.error_message.data.c_str());
                        }
                }
                else
                {
                  res.success.data = false;
                  res.error_message.data = "canopen not initialized, please use init first";
                  ROS_WARN("...recovering canopen not successful. error: %s",res.error_message.data.c_str());
                }
                /* protected region user implementation of service callback for recover end */
                return true;
        }
    


    
    /* protected region user additional functions on begin */
        /* protected region user additional functions end */
    
};