diff --git a/CMakeLists.txt b/CMakeLists.txt
index 514d5d19b38b46f1c27c4b03a124c51b9b29ed06..34853ede58325647eea1682af6090fe21159515a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver move_base_msgs actionlib tf2)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver moby_msgs roscpp move_base_msgs actionlib tf2)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -63,7 +63,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver move_base_msgs actionlib tf2
+ CATKIN_DEPENDS iri_base_driver moby_msgs roscpp move_base_msgs actionlib tf2
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -101,6 +101,8 @@ target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARIES})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} moby_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} roscpp_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} mbf_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} iri_base_driver_generate_messages_cpp)
add_dependencies(${PROJECT_NAME}_gencfg iri_base_driver_generate_messages_py)
diff --git a/README.md b/README.md
index 46e7b1ca6924c6100de0bb79f0a0b19f4d7f15fe..f3db8f517d70b5b42fec729d032e20f08933dc4b 100644
--- a/README.md
+++ b/README.md
@@ -3,6 +3,9 @@
The looming_modbus project description
# ROS Interface
+### Service clients
+ - ~**mode_change** (moby_msgs/ModeChange.srv)
+ - ~**clear_costmaps** (roscpp/Empty.srv)
### Action clients
- ~**move_base** (mbf_msgs/MoveBase.action)
diff --git a/include/looming_modbus_driver_node.h b/include/looming_modbus_driver_node.h
index bf34477f1bd80d334fdab742385504a0038a0c3e..960af72b635ff60d2ffc8515b19090cbe9f2ab94 100644
--- a/include/looming_modbus_driver_node.h
+++ b/include/looming_modbus_driver_node.h
@@ -31,6 +31,8 @@
// [publisher subscriber headers]
// [service client headers]
+#include <moby_msgs/ModeChange.h>
+#include <roscpp/Empty.h>
// [action server client headers]
#include <actionlib/client/simple_action_client.h>
@@ -64,6 +66,12 @@ class LoomingModbusDriverNode : public iri_base_driver::IriBaseNodeDriver<Loomin
// [service attributes]
// [client attributes]
+ ros::ServiceClient mode_change_client_;
+ moby_msgs::ModeChange mode_change_srv_;
+
+ ros::ServiceClient clear_costmaps_client_;
+ roscpp::Empty clear_costmaps_srv_;
+
// [action server attributes]
diff --git a/launch/node.launch b/launch/node.launch
index 83eb3d94504e9ebbe4dda19784be98c7e3838787..89e57bb0a839c0d44b6083a9b6de93b52b9db1e2 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -1,11 +1,13 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="looming_modbus"/>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
- <arg name="config_file" default="$(find looming_modbus)/config/params.yaml"/>
- <arg name="move_base_action" default="~move_base"/>
+ <arg name="node_name" default="looming_modbus"/>
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="config_file" default="$(find looming_modbus)/config/params.yaml"/>
+ <arg name="move_base_action" default="~move_base"/>
+ <arg name="mode_change_service" default="~mode_change"/>
+ <arg name="clear_costmaps_service" default="~clear_costmaps"/>
<node name="$(arg node_name)"
pkg ="looming_modbus"
@@ -14,6 +16,8 @@
launch-prefix="$(arg launch_prefix)">
<rosparam file="$(arg config_file)" command="load"/>
<remap from="~/move_base" to="$(arg move_base_action)"/>
+ <remap from="~mode_change" to="$(arg mode_change_service)"/>
+ <remap from="~clear_costmaps" to="$(arg clear_costmaps_service)"/>
</node>
</launch>
diff --git a/launch/test.launch b/launch/test.launch
index 26c1188f61dc0fff1ccb7ae87b6823aca2b35d3a..81d61328df3963b1f2a2381935251e2ea0e177b2 100644
--- a/launch/test.launch
+++ b/launch/test.launch
@@ -1,19 +1,22 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="output" default="screen"/>
- <arg name="launch_prefix" default=""/>
- <arg name="dr" default="true"/>
- <arg name="config_file" default="$(find looming_modbus)/config/params.yaml"/>
- <arg name="move_base_action" default="~move_base"/>
-
+ <arg name="output" default="screen"/>
+ <arg name="launch_prefix" default=""/>
+ <arg name="dr" default="true"/>
+ <arg name="config_file" default="$(find looming_modbus)/config/params.yaml"/>
+ <arg name="move_base_action" default="~move_base"/>
+ <arg name="mode_change_service" default="~mode_change"/>
+ <arg name="clear_costmaps_service" default="~clear_costmaps"/>
<include file="$(find looming_modbus)/launch/node.launch">
- <arg name="node_name" value="looming_modbus"/>
- <arg name="output" value="$(arg output)"/>
- <arg name="launch_prefix" value="$(arg launch_prefix)"/>
- <arg name="config_file" value="$(arg config_file)"/>
- <arg name="move_base_action" value="$(arg move_base_action)"/>
+ <arg name="node_name" value="looming_modbus"/>
+ <arg name="output" value="$(arg output)"/>
+ <arg name="launch_prefix" value="$(arg launch_prefix)"/>
+ <arg name="config_file" value="$(arg config_file)"/>
+ <arg name="move_base_action" value="$(arg move_base_action)"/>
+ <arg name="mode_change_service" value="$(arg mode_change_service)"/>
+ <arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
</include>
<!--
diff --git a/package.xml b/package.xml
index 6c0a9d608412a0114aa3d000c25e8078c2322c56..42eed8b8cf93983aabca905a349de11383d62a89 100644
--- a/package.xml
+++ b/package.xml
@@ -52,6 +52,8 @@
<build_depend>iri_base_driver</build_depend>
<build_export_depend>iri_base_driver</build_export_depend>
<exec_depend>iri_base_driver</exec_depend>
+ <depend>moby_msgs</depend>
+ <depend>roscpp</depend>
<depend>move_base_msgs</depend>
<depend>actionlib</depend>
<depend>tf2</depend>
diff --git a/src/looming_modbus_driver.cpp b/src/looming_modbus_driver.cpp
index 506a3117a7f8d43475c1c4468d2f4c29d57bca24..c98921ff3bfb89bb7bc247c5b81fae6d068b9d47 100644
--- a/src/looming_modbus_driver.cpp
+++ b/src/looming_modbus_driver.cpp
@@ -33,7 +33,7 @@ bool LoomingModbusDriver::openDriver(void)
if(this->context!=NULL)
{
// create the mapping
- this->mapping = modbus_mapping_new(5,2,sizeof(float)*1.5,sizeof(float)*1.5);
+ this->mapping = modbus_mapping_new(2,5,sizeof(float)*1.5,sizeof(float)*1.5);
if(this->mapping==NULL)
{
ROS_WARN_STREAM("Impossible to create modbus mapping: " << modbus_strerror(errno));
@@ -210,9 +210,9 @@ void LoomingModbusDriver::set_ready(bool status)
{
this->access.enter();
if(status)
- this->mapping->tab_bits[READY_REG]=0x01;
+ this->mapping->tab_input_bits[READY_REG]=0x01;
else
- this->mapping->tab_bits[READY_REG]=0x00;
+ this->mapping->tab_input_bits[READY_REG]=0x00;
this->access.exit();
}
}
@@ -223,9 +223,9 @@ void LoomingModbusDriver::set_navigating(bool status)
{
this->access.enter();
if(status)
- this->mapping->tab_bits[NAVIGATING_REG]=0x01;
+ this->mapping->tab_input_bits[NAVIGATING_REG]=0x01;
else
- this->mapping->tab_bits[NAVIGATING_REG]=0x00;
+ this->mapping->tab_input_bits[NAVIGATING_REG]=0x00;
this->access.exit();
}
}
@@ -236,9 +236,9 @@ void LoomingModbusDriver::set_success(bool status)
{
this->access.enter();
if(status)
- this->mapping->tab_bits[SUCCESS_REG]=0x01;
+ this->mapping->tab_input_bits[SUCCESS_REG]=0x01;
else
- this->mapping->tab_bits[SUCCESS_REG]=0x00;
+ this->mapping->tab_input_bits[SUCCESS_REG]=0x00;
this->access.exit();
}
}
@@ -249,9 +249,9 @@ void LoomingModbusDriver::set_canceled(bool status)
{
this->access.enter();
if(status)
- this->mapping->tab_bits[CANCELLED_REG]=0x01;
+ this->mapping->tab_input_bits[CANCELLED_REG]=0x01;
else
- this->mapping->tab_bits[CANCELLED_REG]=0x00;
+ this->mapping->tab_input_bits[CANCELLED_REG]=0x00;
this->access.exit();
}
}
@@ -262,9 +262,9 @@ void LoomingModbusDriver::set_aborted(bool status)
{
this->access.enter();
if(status)
- this->mapping->tab_bits[ABORTED_REG]=0x01;
+ this->mapping->tab_input_bits[ABORTED_REG]=0x01;
else
- this->mapping->tab_bits[ABORTED_REG]=0x00;
+ this->mapping->tab_input_bits[ABORTED_REG]=0x00;
this->access.exit();
}
}
@@ -273,9 +273,9 @@ bool LoomingModbusDriver::is_start(void)
{
if(this->mapping!=NULL)
{
- if(this->mapping->tab_input_bits[START_REG]!=0x00)
+ if(this->mapping->tab_bits[START_REG]!=0x00)
{
- this->mapping->tab_input_bits[START_REG]=0x00;
+ this->mapping->tab_bits[START_REG]=0x00;
return true;
}
else
@@ -289,9 +289,9 @@ bool LoomingModbusDriver::is_cancel(void)
{
if(this->mapping!=NULL)
{
- if(this->mapping->tab_input_bits[CANCEL_REG]!=0x00)
+ if(this->mapping->tab_bits[CANCEL_REG]!=0x00)
{
- this->mapping->tab_input_bits[CANCEL_REG]=0x00;
+ this->mapping->tab_bits[CANCEL_REG]=0x00;
return true;
}
else
@@ -304,18 +304,18 @@ bool LoomingModbusDriver::is_cancel(void)
void LoomingModbusDriver::set_current_position(float x,float y, float theta)
{
this->access.enter();
- modbus_set_float(x,&this->mapping->tab_registers[CURRENT_POSITION_X]);
- modbus_set_float(y,&this->mapping->tab_registers[CURRENT_POSITION_Y]);
- modbus_set_float(theta,&this->mapping->tab_registers[CURRENT_POSITION_THETA]);
+ modbus_set_float(x,&this->mapping->tab_input_registers[CURRENT_POSITION_X]);
+ modbus_set_float(y,&this->mapping->tab_input_registers[CURRENT_POSITION_Y]);
+ modbus_set_float(theta,&this->mapping->tab_input_registers[CURRENT_POSITION_THETA]);
this->access.exit();
}
void LoomingModbusDriver::get_target_position(float &x,float &y, float &theta)
{
this->access.enter();
- x=modbus_get_float(&this->mapping->tab_input_registers[TARGET_POSITION_X]);
- y=modbus_get_float(&this->mapping->tab_input_registers[TARGET_POSITION_Y]);
- theta=modbus_get_float(&this->mapping->tab_input_registers[TARGET_POSITION_THETA]);
+ x=modbus_get_float(&this->mapping->tab_registers[TARGET_POSITION_X]);
+ y=modbus_get_float(&this->mapping->tab_registers[TARGET_POSITION_Y]);
+ theta=modbus_get_float(&this->mapping->tab_registers[TARGET_POSITION_THETA]);
this->access.exit();
}
diff --git a/src/looming_modbus_driver_node.cpp b/src/looming_modbus_driver_node.cpp
index a737f8258b8b01d6fef3284c2f9e5c36fc4fbeb0..b4df69a0cd5047c0a7174ddc00007d973325bd20 100644
--- a/src/looming_modbus_driver_node.cpp
+++ b/src/looming_modbus_driver_node.cpp
@@ -20,6 +20,9 @@ LoomingModbusDriverNode::LoomingModbusDriverNode(ros::NodeHandle &nh) :
// [init services]
// [init clients]
+ mode_change_client_ = this->private_node_handle_.serviceClient<moby_msgs::ModeChange>("mode_change");
+ clear_costmaps_client_ = this->private_node_handle_.serviceClient<roscpp::Empty>("clear_costmaps");
+
// [init action servers]
@@ -37,6 +40,8 @@ void LoomingModbusDriverNode::mainNodeThread(void)
// [fill msg structures]
// [fill srv structure and make request to the server]
+
+
// [fill action structure and make request to the action server]
//// variable to hold the state of the current goal on the server
@@ -85,7 +90,35 @@ void LoomingModbusDriverNode::mainNodeThread(void)
this->driver_.set_ready(true);
}
if(this->driver_.is_start())
+ {
+ this->mode_change_srv_.request.mode.mode=3;
+ if(this->mode_change_client_.call(this->mode_change_srv_))
+ {
+ if(this->mode_change_srv_.response.accepted)
+ ROS_INFO("LoomingModbusDriverNode:: Navigation mode changed successfully");
+ else
+ {
+ ROS_WARN("LoomingModbusDriverNode:: Impossible to change navigation mode");
+ this->driver_.unlock();
+ return;
+ }
+ }
+ else
+ {
+ ROS_WARN_STREAM("LoomingModbusDriverNode:: Impossible to change navigation mode (no connection to " << this->mode_change_client_.getService() << ")");
+// this->driver_.unlock();
+// return;
+ }
+ if(this->clear_costmaps_client_.call(this->clear_costmaps_srv_))
+ ROS_INFO("LoomingModbusDriverNode:: costmaps cleared");
+ else
+ {
+ ROS_INFO("LoomingModbusDriverNode:: Impossible to clear costmaps");
+ this->driver_.unlock();
+ return;
+ }
this->move_baseMakeActionRequest();
+ }
else if(this->driver_.is_cancel())
move_base_client_.cancelGoal();