Commit e03a21e9 authored by Simon Markus Haller-Seeber's avatar Simon Markus Haller-Seeber
Browse files

Added more of the LSS 4 DOF ROS Control. Can't test it from home - sorry. Hope this helps.

parent 3f4f4c95
......@@ -2,7 +2,6 @@
<arg name="use_gui" default="True" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find lss_4dof_description)/robot/lss_4dof_in_world.urdf.xacro"/>
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" if="$(arg use_gui)" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
......@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(lss_4dof_hw)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17 -fconcepts)
add_compile_options(-std=c++14 -fconcepts)
# Project root directory
GET_FILENAME_COMPONENT(PROJECT_ROOT . ABSOLUTE CACHE INTERNAL "Path prefix for the project")
......@@ -33,11 +33,14 @@ find_package(catkin REQUIRED COMPONENTS
tf
)
find_package(PkgConfig)
pkg_check_modules(SERIAL libserial)
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
#generate_messages(
# DEPENDENCIES
# std_msgs
#)
......@@ -69,6 +72,7 @@ catkin_package(
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
${SERIAL_INCLUDE_DIRS}
)
# Hardware Interface
......@@ -77,7 +81,7 @@ add_library(lss_4dof_hw_interface
)
add_executable(lss_4dof_hw_node ${PROJECT_ROOT}/src/lss_4dof_hw_node.cpp ${PROJECT_ROOT}/src/lss_4dof_hw_interface.cpp ${PROJECT_ROOT}/src/lss_4dof_hw_interface.h)
target_link_libraries(lss_4dof_hw_node lss_4dof_hw_interface ${catkin_LIBRARIES})
target_link_libraries(lss_4dof_hw_node lss_4dof_hw_interface ${catkin_LIBRARIES} ${SERIAL_LDFLAGS})
#############
## Install ##
......
# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 100
# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
joint_trajectory_controller:
type: position_controllers/JointTrajectoryController
joints:
- lss_4dof_axis_1_joint
- lss_4dof_axis_2_joint
- lss_4dof_axis_3_joint
- lss_4dof_axis_4_joint
# Group Position Controllers ---------------------------------------
# Allows to send single ROS msg of Float64MultiArray to all joints
joint_position_controller:
type: position_controllers/JointGroupPositionController
joints:
- lss_4dof_axis_1_joint
- lss_4dof_axis_2_joint
- lss_4dof_axis_3_joint
- lss_4dof_axis_4_joint
# Settings for ros_control control loop and hardware interface
hardware_interface:
loop_hz: 300
cycle_time_error_threshold: 0.01
joints:
- lss_4dof_axis_1_joint
- lss_4dof_axis_2_joint
- lss_4dof_axis_3_joint
- lss_4dof_axis_4_joint
sim_control_mode: 0 # 0: position, 1: velocity
# Joint Names
joints:
- lss_4dof_axis_1_joint
- lss_4dof_axis_2_joint
- lss_4dof_axis_3_joint
- lss_4dof_axis_4_joint
......@@ -9,6 +9,10 @@
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <SerialStream.h>
using namespace LibSerial;
#include "lss_4dof_hw_interface.h"
#include <iostream> // std::cout, std::endl
# define M_PI 3.14159265358979323846 /* pi */
......@@ -20,8 +24,6 @@ using namespace std::literals;
namespace lss_4dof_hw_interface
{
// Connect to LSS Arm
//SerialPort serial_port { "/dev/ttyUSB0", 115200 };
LSS4DOFHWInterface::LSS4DOFHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model) :
name_("lss_4dof_hw_interface"),
......@@ -45,9 +47,23 @@ namespace lss_4dof_hw_interface
void LSS4DOFHWInterface::init()
{
// Libserial Documentation: https://libserial.readthedocs.io/en/latest/description.html
SerialStream serial;
serial.Open("/dev/ttyUSB0");
serial.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);
serial.SetBaudRate(SerialStreamBuf::DEFAULT_BAUD);
serial.SetBaudRate(SerialStreamBuf::BAUD_115200 );
serial.SetNumOfStopBits(1);
serial.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);
if(serial.good()){
ROS_INFO("Successfully connected to the LSS 4 DoF Arm");
} else{
ROS_ERROR("Error: could not open serial port. Can't use LSS 4 DoF Arm");
}
ROS_INFO("LSS 4 DOF ready");
// maybe -1 becuase of the gripper?
num_joints_ = joint_names_.size();
// Status
......@@ -99,20 +115,68 @@ namespace lss_4dof_hw_interface
void LSS4DOFHWInterface::read(ros::Duration &elapsed_time)
{
// ToDo current position from Arm
//joint_position_.clear();
std::string myposstr="";
double pos_double;
joint_position_.clear();
// Query position (https://www.robotshop.com/info/wiki/lynxmotion/view/lynxmotion-smart-servo/lss-communication-protocol/)
// This should compile with libserial-dev (Version >=1.0. Note: Ubuntu 18.04 uses libserial-dev 0.6)
// This is an untest code stub just to get going
//joint_position_.push_back();
//joint_position_.push_back(v_.y);
/*
serial << "#0QD" << std::endl ;
myposstr << serial.Readline() ;
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*0.017453 ;
myposstr="";
joint_position_.push_back(pos_double);
serial << "#1QD" << std::endl ;
myposstr << serial.Readline() ;
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*0.017453;
myposstr="";
joint_position_.push_back(pos_double);
serial << "#2QD" << std::endl ;
myposstr << serial.Readline() ;
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*0.017453;
myposstr="";
joint_position_.push_back(pos_double);
serial << "#3QD" << std::endl ;
myposstr << serial.Readline() ;
// Diveded by 10 because we get 1/10 degree and converted tor rad
pos_double = std::stod(myposstr.substr(4))/10*0.017453;
myposstr="";
joint_position_.push_back(pos_double);
*/
// maybe also fill in the 5th joint gripper??
// Gripper could be handled by a service call which sets a target global varibale for the write
}
void LSS4DOFHWInterface::write(ros::Duration &elapsed_time)
{
// Safety
ROS_INFO_STREAM_NAMED(name_, "Update L:" << joint_velocity_command_.at(0) << " R:" << joint_velocity_command_.at(1) );
//ROS_INFO_STREAM_NAMED(name_, "Update L:" << joint_position_command_.at(0) << " R:" << joint_position_command_.at(1) ... 2 ... 4 );
enforceLimits(elapsed_time);
// ToDo Write Position to Motors
// Write position (https://www.robotshop.com/info/wiki/lynxmotion/view/lynxmotion-smart-servo/lss-communication-protocol/)
// This is an untest code stub (just to get going)
// First convert joint pos command to degree, then times 10 (for the motor command);
// use e.g. floor to cut of smaller than 1/10 degree, and send to motor
serial << "#0D" << floor(joint_position_command_.at(0)*(180.0/M_PI)*10) << std::endl ;
serial << "#1D" << floor(joint_position_command_.at(1)*(180.0/M_PI)*10) << std::endl ;
serial << "#2D" << floor(joint_position_command_.at(2)*(180.0/M_PI)*10) << std::endl ;
serial << "#3D" << floor(joint_position_command_.at(3)*(180.0/M_PI)*10) << std::endl ;
}
......@@ -328,5 +392,6 @@ namespace lss_4dof_hw_interface
void LSS4DOFHWInterface::disconnect()
{
serial.Close();
}
}
......@@ -5,6 +5,7 @@
#include <boost/scoped_ptr.hpp>
#include <iostream>
#include <cstdlib>
#include <SerialStream.h>
//#include <future>
//#include <thread>
......@@ -35,6 +36,7 @@ namespace lss_4dof_hw_interface
*/
LSS4DOFHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model = NULL);
/** \brief Destructor */
virtual ~LSS4DOFHWInterface() {}
......@@ -91,8 +93,11 @@ namespace lss_4dof_hw_interface
/** \brief Helper for debugging a joint's command */
std::string printCommandHelper();
// Serial connection
LibSerial::SerialStream serial;
protected:
/** \brief Sphero RVR interface */
/** \brief LSS 4DOF interface */
LSS4DOFHWInterface *lss_4dof;
/** \brief Get the URDF XML from the parameter server */
......@@ -101,6 +106,7 @@ namespace lss_4dof_hw_interface
// Short name of this class
std::string name_;
// Startup and shutdown of the internal node inside a roscpp program
ros::NodeHandle nh_;
......
<launch>
<arg name="use_gui" default="True" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find minibot)/model/minibot_in_world.urdf.xacro"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" if="$(arg use_gui)" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
</launch>
......@@ -2,7 +2,6 @@
<arg name="use_gui" default="True" />
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find sphero_rvr_description)/robot/sphero_rvr_in_world.urdf.xacro"/>
<node pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" name="joint_state_publisher_gui" output="screen" if="$(arg use_gui)" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>
......@@ -28,6 +28,7 @@
- build-essential
- git
- dkms
- rsync
- name: Add Nvidia apt signing key to the debian keyring file
apt_key:
......@@ -110,7 +111,7 @@
- libpython2-stdlib
- python2
- python2-minimal
- python-is-python2
# - python-is-python2
- libgstreamer1.0-0
- libgstreamer-plugins-bad1.0-0
- libpangocairo-1.0-0
......@@ -176,13 +177,13 @@
- ros-noetic-ros-base
- ros-noetic-urg-node
- ros-noetic-teleop-twist-keyboard
- ros-noetic-joystick-drivers
# - ros-noetic-joystick-drivers
- ros-noetic-ros-control
- ros-noetic-control-msgs
- ros-noetic-control-toolbox
- ros-noetic-diff-drive-controller
- ros-noetic-rosparam-shortcuts
- ros-noetic-teleop-twist-joy
# - ros-noetic-teleop-twist-joy
- ros-noetic-librealsense2
- ros-noetic-realsense2-camera
- ros-noetic-realsense2-description
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment